function [Xmat,pobjround,info] = local_search_sra(Zmat,C,rrPar,opt,roundonly)
%% local search for single rotation averaging
%% used as a subroutine for STRIDE
if nargin < 5
    roundonly = false;
end
if nargin < 4
    % default round the first two eigenvectors
    opt = [1,2];
end

if roundonly
    [R,theta] = round_sra(Zmat,[1]);
    xtld      = lift_sra(R(:),theta);
    pobjround = xtld{1}' * C{1} * xtld{1};
    Xmat      = rank_one_lift(xtld);
else
    [R,theta] = round_sra(Zmat,opt);
    Zmatround = {};
    pobjround = zeros(length(opt),1);
    for i = 1:length(opt)
        Ri              = squeeze(R(:,:,i));
        thetai          = theta(:,i);
        [Ztmp,~,~,pobjtmp] = nlp_sra(C,Ri,thetai);
        Zmatround{end+1} = Ztmp;
        pobjround(i) = pobjtmp;
    end
    pobjs            = pobjround;
    [pobjround,idx]  = min(pobjround);
    if pobjround == inf
        fprintf('        NLP fails to find a good solution, return a rounded solution only.\n');
        Ropt         = squeeze(R(:,:,1));
        ropt         = Ropt(:);
        thetaopt     = theta(:,1);
        xopt         = lift_sra(ropt,thetaopt);
        Xmat         = rank_one_lift(xopt);
        pobjround    = xopt{1}'*C{1}*xopt{1};
        nlpsuccess   = false;
    else
        Xmat = Zmatround{idx};
        nlpsuccess   = true;
    end
end

if nargout > 2
    info.minidx     = idx;
    info.nlpsuccess = nlpsuccess;
    info.pobjs      = pobjs;
    info.diffpobj   = pobjs(1) - pobjround;
end

end

function f = sra_cost(x,C)
R       = x.A;
r       = R(:);
theta   = x.B;
theta   = theta(:);
v       = lift_sra(r,theta);
f       = v{1}'*C*v{1};
end

function g = sra_egrad(x,C)
R       = x.A;
r       = R(:);
theta   = x.B;
theta   = theta(:);
N       = length(theta);
v       = lift_sra(r,theta);
v       = v{1};

gv      = 2 * v' * C; % 1 x n

vdr     = [sparse(1,9);...
           speye(9);...
           sparse(N,9);...
           kron(theta,speye(9))]; % n x 9
gr      = (gv * vdr)'; % 9 x 1

vdtheta = [sparse(10,N);...
           speye(N);...
           kron(speye(N),r)]; % n x N
gtheta  = (gv * vdtheta); % 1 x N

g.A     = reshape(gr,3,3);
g.B     = gtheta;
end

function [Xmat,Ropt,thetaopt,fopt] = nlp_sra(C,R,theta)
N           = length(theta);
elements.A  = rotationsfactory(3,1);
elements.B  = obliquefactory(1,N);
manifold    = productmanifold(elements);

problem.M   = manifold;

warning('off', 'manopt:getHessian:approx')
% Define the problem cost function and its Euclidean gradient.
problem.cost  = @(x) sra_cost(x,C{1});
problem.egrad = @(x) sra_egrad(x,C{1});

% Numerically check gradient consistency (optional).
% checkgradient(problem);
% Solve.
x0.A                 = R;
x0.B                 = theta';
options.verbosity    = 0;
options.tolgradnorm  = 1e-6;
[xopt, fopt, output, options] = trustregions(problem,x0,options);
Ropt = xopt.A;
thetaopt = xopt.B';
constraintviolationR          = norm(Ropt*Ropt'-eye(3),'fro');
constraintviolationtheta      = max(abs(thetaopt.^2 - 1));
firstorderopt                 = output(end).gradnorm;
fprintf('        MANOPT: itr: %3d, constraint violation: %3.2e, %3.2e, gradnorm: %3.2e, cost: %3.8e.\n',...
    length(output),constraintviolationR,constraintviolationtheta,firstorderopt,fopt);
if max(constraintviolationR,constraintviolationtheta) < 1e-8 ...
        && firstorderopt < 1e-6
    % Do nothing
else
    fopt    = inf;
end
vnew     = lift_sra(Ropt(:),thetaopt(:));
Xmat     = rank_one_lift(vnew);
end


function Select = subsetSelection(Obj,Objhat,N)

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Tomoaki Takagi

    Len = length(Obj);
    Obj = [Obj;Objhat];

    %% Select the representative objective vector set
    LpNormD = pdist2(Obj,Obj);
    Select = false(1,size(Obj,1));
    Select(1:Len) = true;
    % Greedy inclusion distance-based subset slection
    while sum(Select) < N
        Remain   = find(~Select);
        [~, rho] = max(min(LpNormD(Remain,Select),[],2));
        Select(Remain(rho)) = true;
    end
    Select = Select(Len+1:end);
end


function eciVector = rotateVectorFromRsw2Eci(rswVector, rVect, vVect)
    R = normVector(rVect);
    W = normVector(crossARH(rVect, vVect));
    S = normVector(crossARH(W,R));

    RSW2ECIRotMat = [R,S,W]';

    eciVector = RSW2ECIRotMat \ rswVector;
end


function tr=tau_rest(veh,p)

% tr=tau_rest(veh,p); calculates restoring forces from
% vehicle variables and generalized position p

% Hydrostatic force and moment
FB_e=-veh.vol*veh.rho*veh.g_e;
FB_b=rpy2R_eb(p(4:6))*FB_e;
MB_b=vp(veh.B_b,FB_b);
tb=[FB_b;MB_b];

% Gravitational force and moment
FG_e=veh.m*veh.g_e;
FG_b=rpy2R_eb(p(4:6))*FG_e;
MG_b=vp(veh.G_b,FG_b);
tg=[FG_b;MG_b];

tr=tb+tg;


function [gx] = g_LinDecomp(x,P,u,in)
X = NaN(in.size.X(1),in.size.X(2));
Y = NaN(in.size.Y(1),in.size.Y(2));
for i=1:in.n
    X(:,i) = P(in.ind(i).X);
    Y(i,:) = P(in.ind(i).Y)';
end
gx = VBA_vec(X*Y) + P(in.ind0);


classdef CEC2020_F9 < PROBLEM
% <single> <real>
% Composition function 2

%------------------------------- Reference --------------------------------
% C .T. Yue, K. V. Price, P. N. Suganthan, J. J. Liang, M. Z. Ali, B. Y.
% Qu, N. H. Awad, and P. P Biswas, Problem definitions and evaluation
% criteria for the CEC 2020 special session and competition on single
% objective bound constrained numerical optimization, Zhengzhou University,
% China and Nanyang Technological University, Singapore, 2019.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        O;      % Optimal decision vector
        Mat;	% Rotation matrix
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2020.mat'),'Data');
            obj.O = Data{9}.o;
            obj.M = 1;
            if isempty(obj.D) || obj.D < 10
                obj.D   = 5;
                obj.Mat = Data{9}.M_5;
            elseif obj.D < 15
                obj.D   = 10;
                obj.Mat = Data{9}.M_10;
            elseif obj.D < 20
                obj.D   = 15;
                obj.Mat = Data{9}.M_15;
            else
                obj.D   = 20;
                obj.Mat = Data{9}.M_20;
            end
            obj.lower    = zeros(1,obj.D) - 100;
            obj.upper    = zeros(1,obj.D) + 100;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            lambda = [10 1e-6 10 1];
            delta  = [10 20 30 40];
            bias   = [0 100 200 300];
            func   = {@Ackley,@Elliptic,@Griewank,@Rastrigin};
            W      = zeros(size(PopDec,1),4);
            F      = zeros(size(W));
            for i = 1 : size(W,2)
                tmp    = sum((PopDec-repmat(obj.O(i,1:size(PopDec,2)),size(PopDec,1),1)).^2,2);
                W(:,i) = 1./(sqrt(tmp)+1e-10).*exp(-tmp/2/obj.D/delta(i)^2);
                F(:,i) = func{i}((PopDec-repmat(obj.O(i,1:size(PopDec,2)),size(PopDec,1),1))*obj.Mat((i-1)*obj.D+1:i*obj.D,:)');
            end
            W = W./repmat(sum(W,2),1,size(W,2));
            PopObj = 2400 + sum(W.*(repmat(lambda,size(F,1),1).*F+repmat(bias,size(F,1),1)),2);
        end
    end
end

function F = Ackley(X)
    F = -20*exp(-0.2*sqrt(mean(X.^2,2))) - exp(mean(cos(2*pi*X),2)) + 20 + exp(1);
end

function F = Elliptic(X)
    F = sum((1e6).^(repmat(0:size(X,2)-1,size(X,1),1)/(size(X,2)-1+1e-6)).*X.^2,2);
end

function F = Griewank(X)
    X = 6*X;
    F = sum(X.^2,2)/4000 - prod(cos(X)./repmat(sqrt(1:size(X,2)),size(X,1),1),2) + 1;
end

function F = Rastrigin(X)
    X = 0.0512*X;
    F = sum(X.^2-10*cos(2*pi*X)+10,2);
end


function [xUpdate,SUpdate,innov,Szz,W]=sqrtCubKalUpdate(xPred,SPred,z,SR,h,xi,w,innovTrans,measAvgFun,stateDiffTrans,stateTrans)
%SQRTCUBKALUPDATE Perform the measurement update step in the square root
%                 cubature Kalman filter with additive measurement noise.
%                 Unlike the non-square root version, the covariance update
%                 is such that finite precision problems cannot lead to a
%                 non-positive (semi-)definite covariance. However, unlike
%                 the non-square root version, all of the cubature weights
%                 must be positive.
%
%INPUTS: xPred The xDim X 1 predicted target state.
%        SPred The xDim X xDim lower-triangular square root predicted state
%              covariance matrix.
%            z The zDim X 1  measurement vector.
%           SR The zDim X zDim lower-triangular square root of the
%              measurement covariance matrix in the native coordinate
%              system of the measurement.
%            h A function handle for the measurement function that the
%              state as its argument.
%           xi An xDim X numCubPoints matrix of cubature points. If this
%              and the next parameter are omitted or empty matrices are
%              passed, then fifthOrderCubPoints(xDim). It is suggested that
%              xi and w be provided to avoid needless recomputation of the
%              cubature points.
%            w A numCubPoints X 1 vector of the weights associated with the
%              cubature points.
%   innovTrans An optional function handle that computes and optionally
%              transforms the value of the difference between the
%              observation and any predicted points. This is called as
%              innovTrans(a,b) and the default if omitted or an empty
%              matrix is passed is @(a,b)bsxfun(@minus,a,b). This must be
%              able to handle sets of values. For a zDimX1 measurement,
%              either of the inputs could be zDimXN in size while one of
%              the inputs could be zDimX1 in size.  This only needs to be
%              supplied when a measurement difference must be restricted
%              to a certain range. For example, the innovation between two
%              angles will be 2*pi if one angle is zero and the other
%              2*pi, even though they are the same direction. In such an
%              instance, a function handle to the
%              wrapRange(bsxfun(@minus,a,b),-pi,pi) function with the
%              appropriate parameters should be passed for innovTrans.
%   measAvgFun An optional function handle that, when given N measurement
%              values with weights, produces the weighted average. This
%              function only has to be provided if the domain of the
%              measurement is not linear. For example, when averaging
%              angular values, then the function meanAng should be used.
% stateDiffTrans An optional function handle that takes an xDimXN matrix of
%              N differences between states and transforms them however
%              might be necessary. If not transformation is necessary, this
%              parameter can be omitted or an empty matrix passed.
%   stateTrans An optional function that takes a state estimate and
%              transforms it. This is useful if one wishes the elements of
%              the state to be bound to a certain domain. For example, if
%              an element of the state is an angle, one might generally
%              want to bind it to the region +/-pi.
%
%OUTPUTS: xUpdate The xDim X 1 updated state vector.
%         SUpdate The updated xDim X xDim lower-triangular square root
%                 state covariance matrix.
%      innov, Szz The zDimX1 innovation and the zDimXzDim square root
%                 innovation covariance matrix are returned in case one
%                 wishes to analyze the consistency of the estimator or use
%                 those values in gating or likelihood evaluation.
%               W The xDimXzDim gain used in the update. This can be
%                 useful when gating and using the function
%                 calcMissedGateCov.
%
%If the function h needs additional parameters beyond the state, then the
%parameters can be passed by using an anonymous function as the function
%handle. For example, suppose that the measurement function is measFunc and
%it needs the additional parameters param1 and param2. In this instance,
%rather than using
%h=@measFunc
%one should use
%h=@(x)measFunc(x,param1,param2)
%This way, every time sqrtCubKalUpdate calls measFunc (via h) with a
%different x, those two parameters are always passed.
%
%The mathematics behind the function sqrtCubKalUpdate are described in more
%detail in Section IX of [1] and in [2]. Note that this is essentially one
%type of square root "unscented Kalman filter" with additive noise. One
%simply has to provide the filter with the appropriate cubature points and
%weights.
%
%The optional parameters innovTrans and measAvgFun are not described in
%references [1] and [2], but allow for possible modifications to the filter
%as described in [3] The parameters have been added to allow the filter to
%be used with angular quantities. For example, if the measurement consisted
%of range and angle, z=[r;theta], then
%innovTrans=@(a,b)[bsxfun(@minus,a(1,:),b(1,:));
%                  wrapRange(bsxfun(@minus,a(2,:),b(2,:)),-pi,pi)];
%measAvgFun=@(z,w)[calcMixtureMoments(z(1,:),w);
%                  meanAng(z(2,:),w')];
%should be used to approximately deal with the circular nature of the
%measurements.
%
%REFERENCES:
%[1] D. F. Crouse , "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems Magazine,
%    vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%[2] I. Arasaratnam and S. Haykin, "Cubature Kalman filters," IEEE
%    Transactions on Automatic Control, vol. 54, no. 6, pp. 1254-1269,
%    Jun. 2009.
%[3] D. F. Crouse, "Cubature/ unscented/ sigma point Kalman filtering with
%    angular measurement models," in Proceedings of the 18th International
%    Conference on Information Fusion, Washington, D.C., 6-9 Jul. 2015.
%
%July 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    xDim=size(xPred,1);

    if(nargin<6||isempty(xi))
        [xi,w]=fifthOrderCubPoints(xDim);
    end

    if(nargin<8||isempty(innovTrans))
        %The function just returns the input.
        innovTrans=@(a,b)bsxfun(@minus,a,b);
    end

    if(nargin<9||isempty(measAvgFun))
        measAvgFun=@(zPoints,w)calcMixtureMoments(zPoints,w);
    end

    if(nargin<10||isempty(stateDiffTrans))
        stateDiffTrans=@(x)x;
    end

    if(nargin<11||isempty(stateTrans))
        stateTrans=@(x)x;
    end

    zDim=size(z,1);
    numCubPoints=size(xi,2);
    sqrtW=sqrt(w);
    %Calculate the updated estimates

    %Predicted cubature state points
    xPredPoints=stateTrans(transformCubPoints(xi,xPred,SPred));

    %Predicted, centered cubature state points
    xPredCenPoints=bsxfun(@times,stateDiffTrans(bsxfun(@minus,xPredPoints,xPred)),sqrtW');

    %Predicted cubature measurement points
    zPredPoints=zeros(zDim,numCubPoints);
    for curP=1:numCubPoints
        zPredPoints(:,curP)=h(xPredPoints(:,curP));
    end

    %Measurement prediction.
    zPred=measAvgFun(zPredPoints,w);

    %Centered, predicted cubature measurement points, transformed as
    %necessary to keep the values within a desired range.
    zPredCenPoints=bsxfun(@times,innovTrans(zPredPoints,zPred),sqrtW');

    %Root innovation covariance
    Szz=tria([zPredCenPoints,SR]);

    %The cross covariance.
    Pxz=xPredCenPoints*zPredCenPoints';

    %The filter gain
    W=(Pxz/Szz')/Szz;

    %The innovation, transformed as necessary to keep values in a desired
    %range.
    innov=innovTrans(z,zPred);

    %Updated state estimate
    xUpdate=stateTrans(xPred+W*innov);

    %Updated state root covariance
    SUpdate=tria([stateDiffTrans(xPredCenPoints-W*zPredCenPoints),W*SR]);
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


%
% This script will take a given set of fiber groups loaded in dtiFiberUI,
% which cross the callosum, and find 11 points along each fiber that are
% cented around the midsagital point of each path (5 steps to each side
% of the center point).
%
% For each of the fiber groups loaded it will then return the means for FA, MD,
% RD, AD, and selected background image value, in a Nx5 array stored in the
% variable mnVal. Currently this script is set up for use with fmap background
% images.
%
% The standard deviation will also be computed for each of the 5 vals and
% stored in a Nx5 array in the variable sdVal.
%
% The names and sequence of fiber grous can be found in fg.name
%
%
% HISTORY:
% 2008.08.08 RFD Wrote it.

%dataDir = '/biac3/wandell4/data/reading_longitude/dti_adults/ah080521_sense/dtiss06';
dataDir = '/biac3/wandell4/data/reading_longitude/dti_adults/rfd050504_SENSE/dti23';
roiDir = fullfile(fileparts(dataDir),'ROIs');

handles = guidata(gcf);
nSteps = 5;

fg = handles.fiberGroups;
nFg = numel(fg);

% Create a legend for the fiber groups
for(ii=1:nFg)
    fgCol(ii,:) = fg(ii).colorRgb;
    fgStr{ii} = fg(ii).name;
end
legImg = ones(18*nFg-1,16,3);
for(ii=1:nFg)
    for(jj=1:3)
    	yPos = (ii-1)*18+1;
    	legImg(yPos:yPos+16,:,jj) = fgCol(ii,jj)./255;
    end
end
figure(88); image(legImg); axis equal off tight;
set(gca,'units','pixels','position',[8 10 size(legImg,2) size(legImg,1)]);
for(ii=1:nFg)
  text(18,(ii-1)*18+9, strrep(fgStr{ii},'_',' '),'FontSize',10);
end

% Get the callosal ROI
ccRoi = dtiReadRoi(fullfile(roiDir,'CC'));
minDist = 0.87;

h = mrvWaitbar(0,'Processing fibers...');
msCoords = [];
for(ii=1:nFg)
    fiberCoords{ii} = [];
    for(jj=1:numel(fg(ii).fibers))
        fc = fg(ii).fibers{jj};
        % first find those points that are within the CC ROI
        [indices, bestSqDist] = nearpoints(fc, ccRoi.coords');
        keepAll = bestSqDist<=minDist^2;
        if(any(keepAll))
            midSagPos = min(abs(fc(1,keepAll)));
            midSagInd = find(abs(fc(1,:))==midSagPos & keepAll);
            if(numel(midSagInd)~=1)
                disp('ignoring fiber');
            else
                msCoords = horzcat(msCoords, fc(:,midSagInd));
                midSagInds = [midSagInd-nSteps:midSagInd+nSteps];
                if(midSagInds(1)>1 && midSagInds(end)<size(fc,2))
                    fiberCoords{ii} = horzcat(fiberCoords{ii}, fc(:,midSagInds));
                end
            end
        end
    end
    mrvWaitbar(ii/nFg,h);
end
close(h);
figure; subplot(2,1,1);
c = horzcat(fiberCoords{:});
plot(c(2,:),c(3,:),'b.');
axis equal;
subplot(2,1,2);
plot(msCoords(2,:),msCoords(3,:),'r.'); hold on;
plot(ccRoi.coords(:,2),ccRoi.coords(:,3),'ko');
hold off; axis equal tight;

% Extract the values and summarize the callosal segments
bg = handles.bg(dtiGet(handles,'curbgnum'));
bg.img = bg.img.*(bg.maxVal-bg.minVal)+bg.minVal;
for(ii=1:nFg)
    [ev1,ev2,ev3] = dtiGetValFromTensors(handles.dt6, fiberCoords{ii}, ...
        inv(handles.xformToAcpc), 'eigvals', 'nearest');
    [fa,md,rd,ad] = dtiComputeFA(horzcat(ev1,ev2,ev3));
    gv = fa<1;
    bgCoords = round(mrAnatXformCoords(inv(bg.mat),fiberCoords{ii}));
    bgInds = sub2ind(size(bg.img),bgCoords(:,1),bgCoords(:,2),bgCoords(:,3));
    bgVals = bg.img(bgInds);
    % get rid of bad values (values that are exactly zero)
    bgVals = bgVals(bgVals>0 & gv);
    mnVal(ii,:) = [mean(fa(gv)),mean(md(gv)),mean(rd(gv)),mean(ad(gv)),mean(bgVals)];
    sdVal(ii,:) = [std(fa(gv)),std(md(gv)),std(rd(gv)),std(ad(gv)),std(bgVals)];
end

error('stop here');

handles = guidata(gcf);
ccfg = handles.fiberGroups(end);
% concatenate all fibers into one big list of points
ccCoords = horzcat(ccfg.fibers{:});
[ev1,ev2,ev3] = dtiGetValFromTensors(handles.dt6, ccCoords, inv(handles.xformToAcpc), 'eigvals', 'nearest');
[fa,md,rd,ad] = dtiComputeFA(horzcat(ev1,ev2,ev3));
bgCoords = round(mrAnatXformCoords(inv(bg.mat),ccCoords));
bgInds = sub2ind(size(bg.img),bgCoords(:,1),bgCoords(:,2),bgCoords(:,3));
bgVals = bg.img(bgInds);
% get rid of bad values (values that are exactly zero)
gv = fa<1 & md>0.5 & md<1.2 & bgVals>0 & bgVals<0.27;
figure;plot(md(gv),bgVals(gv),'.')


clear all;
% -------------------------------------------------------------------------
%   Description:
%       Script to demo LapSRN for one image
%
%   Citation:
%       Deep Laplacian Pyramid Networks for Fast and Accurate Super-Resolution
%       Wei-Sheng Lai, Jia-Bin Huang, Narendra Ahuja, and Ming-Hsuan Yang
%       IEEE Conference on Computer Vision and Pattern Recognition (CVPR), 2017
%
%   Contact:
%       Wei-Sheng Lai
%       wlai24@ucmerced.edu
%       University of California, Merced
% -------------------------------------------------------------------------

img_filename = 'emma.jpg';

%% parameters
scale  = 4; % SR upsampling scale
gpu    = 1; % GPU ID

%% setup paths
addpath(genpath('utils'));
addpath(fullfile(pwd, 'matconvnet/matlab'));
vl_setupnn;

%% Load pretrained odel
model_filename = fullfile('pretrained_models', sprintf('LapSRN_x%d.mat', scale));

fprintf('Load %s\n', model_filename);
net = load(model_filename);
net = dagnn.DagNN.loadobj(net.net);
net.mode = 'test' ;

if( gpu ~= 0 )
    gpuDevice(gpu)
    net.move('gpu');
end

%% Load GT image
fprintf('Load %s\n', img_filename);
img_GT = im2double(imread(img_filename));
img_GT = mod_crop(img_GT, scale);

%% Generate LR image
img_LR = imresize(img_GT, 1/scale);

%% apply LapSRN
fprintf('Apply LapSRN for %dx SR\n', scale);
img_HR = SR_LapSRN(img_LR, net, scale, gpu);

%% show results
img_LR = imresize(img_LR, scale);
figure, imshow(cat(2, img_LR, img_HR, img_GT));
title(sprintf('Bicubic %dx    |    LapSRN %dx    |    Ground Truth', scale, scale));


function [varargout] = likGamma(link, hyp, y, mu, s2, inf, i)

% likGamma - Gamma likelihood function for strictly positive data y. The
% expression for the likelihood is
%   likGamma(f) = al^al*y^(al-1)/gamma(al) * exp(-y*al/mu) / mu^al with
% mean=mu and variance=mu^2/al where mu = g(f) is the Gamma intensity, f is a
% Gaussian process, y is the strictly positive data. Hence, we have -- with
% log(Zy) = log(gamma(al)) - al*log(al) + (1-al)*log(y)
%   llik(f) = log(likGamma(f)) = -al*( log(g(f)) + y/g(f) ) - log(Zy).
% The larger one chooses al, the stronger the likelihood resembles a Gaussian
% since skewness = 2/sqrt(al) and kurtosis = 6/al.
%
% We provide two inverse link functions 'exp' and 'logistic':
%   g(f) = exp(f) and g(f) = log(1+exp(f))).
% The link functions are located at util/glm_invlink_*.m.
%
% Note that for neither link function the likelihood lik(f) is log concave.
%
% The hyperparameters are:
%
% hyp = [  log(al)  ]
%
% Several modes are provided, for computing likelihoods, derivatives and moments
% respectively, see likFunctions.m for the details. In general, care is taken
% to avoid numerical issues when the arguments are extreme.
%
% See also LIKFUNCTIONS.M.
%
% Copyright (c) by Hannes Nickisch, 2013-10-16.

if nargin<4, varargout = {'1'}; return; end   % report number of hyperparameters

al = exp(hyp);

if nargin<6                              % prediction mode if inf is not present
  if numel(y)==0,  y = zeros(size(mu)); end
  s2zero = 1; if nargin>4, if norm(s2)>0, s2zero = 0; end, end         % s2==0 ?
  if s2zero                                                    % log probability
    lg = g(mu,link);
    lZy = gammaln(al) - al*log(al) + (1-al)*log(y);     % normalisation constant
    lp = -al*(lg+y./exp(lg)) - lZy;
  else
    lp = likGamma(link, hyp, y, mu, s2, 'infEP');
  end
  ymu = {}; ys2 = {};
  if nargout>1                                 % compute y moments by quadrature
    n = max([length(y),length(mu),length(s2)]); on = ones(n,1);
    N = 20; [t,w] = gauher(N); oN = ones(1,N); lw = ones(n,1)*log(w');
    mu = mu(:).*on; sig = sqrt(s2(:)).*on;                        % vectors only
    lg = g(sig*t'+mu*oN,link);
    ymu = exp(logsumexp2(lg+lw));     % first moment using Gaussian-Hermite quad
    if nargout>2
      elg = exp(lg);
      yv = elg.^2/al;                  % second y moment from Gamma distribution
      ys2 = (yv+(elg-ymu*oN).^2)*w;
    end
  end
  varargout = {lp,ymu,ys2};
else
  switch inf
  case 'infLaplace'
    [lg,dlg,d2lg,d3lg] = g(mu,link); elg = exp(lg);
    if nargin<7                                             % no derivative mode
      lZy = gammaln(al) - al*log(al) + (1-al)*log(y);   % normalisation constant
      lp = -al*(lg+y./elg) - lZy;
      dlp = {}; d2lp = {}; d3lp = {};                         % return arguments
      if nargout>1
        dlp = -al*dlg.*(1-y./elg);           % dlp, derivative of log likelihood
        if nargout>2                    % d2lp, 2nd derivative of log likelihood
          d2lp = -al*d2lg.*(1-y./elg) - al*dlg.*dlg.*y./elg;
          if nargout>3                  % d3lp, 3rd derivative of log likelihood
            d3lp = -al*d3lg.*(1-y./elg) + al*dlg.*(dlg.*dlg-3*d2lg).*y./elg;
          end
        end
      end
      varargout = {lp,dlp,d2lp,d3lp};
    else                                                       % derivative mode
      dlZy = al*psi(0,al) - al*(log(al) + 1 + log(y));
      lp_dhyp = -al*(lg+y./elg) - dlZy; % derivative of log likelihood w.r.t. al
      dlp_dhyp = -al*dlg.*(1-y./elg);                         % first derivative
      d2lp_dhyp = -al*d2lg.*(1-y./elg) - al*dlg.*dlg.*y./elg;  % and also second
      varargout = {lp_dhyp,dlp_dhyp,d2lp_dhyp};
    end

  case 'infEP'
    if nargin<7                                             % no derivative mode
      % Since we are not aware of an analytical expression of the integral,
      % we use quadrature.
      varargout = cell(1,nargout);
      [varargout{:}] = lik_epquad({@likGamma,link},hyp,y,mu,s2);
    else                                                       % derivative mode
      varargout = {[]};                                     % deriv. wrt hyp.lik
    end

  case 'infVB'
    error('infVB not supported')
  end
end

% compute the log intensity using the inverse link function
function varargout = g(f,link)
  varargout = cell(nargout, 1);  % allocate the right number of output arguments
  if strcmp(link,'exp')
    [varargout{:}] = glm_invlink_exp(f);
  else
    [varargout{:}] = glm_invlink_logistic(f);
  end


% Test file for chebtech2 constructor.
% Here, we check populate().  (This function is not user-facing.)

function pass = test_constructor(pref)

% Get preferences:
if ( nargin < 1 )
    pref = chebtech.techPref();
end
% Set the tolerance:
tol = 100*pref.chebfuneps;

% Initialize with default data:
data = chebtech.parseDataInputs(struct());

%%
% Test on a scalar-valued function:
pref.extrapolate = 0;
pref.refinementFunction = 'nested';
f = @(x) sin(x);
g = populate(chebtech2, f, data, pref);
x = chebtech2.chebpts(length(g.coeffs));
values = g.coeffs2vals(g.coeffs);
pass(1) = norm(f(x) - values, inf) < tol;
pass(2) = abs(vscale(g) - sin(1)) < eps && g.ishappy && eps < tol;

pref.extrapolate = 1;
pref.refinementFunction = 'nested';
g = populate(chebtech2, f, data, pref);
x = chebtech2.chebpts(length(g.coeffs));
values = g.coeffs2vals(g.coeffs);
pass(3) = norm(f(x) - values, inf) < tol;
pass(4) = norm(vscale(g) - sin(1), inf) < tol && logical(eps);

pref.extrapolate = 0;
pref.refinementFunction = 'resampling';
g = populate(chebtech2, f, data, pref);
x = chebtech2.chebpts(length(g.coeffs));
values = g.coeffs2vals(g.coeffs);
pass(5) = norm(f(x) - values, inf) < tol;
pass(6) = abs(vscale(g) - sin(1)) < eps && logical(eps);

pref.extrapolate = 1;
pref.refinementFunction = 'resampling';
g = populate(chebtech2, f, data, pref);
x = chebtech2.chebpts(length(g.coeffs));
values = g.coeffs2vals(g.coeffs);
pass(7) = norm(f(x) - values, inf) < tol;
pass(8) = norm(vscale(g) - sin(1), inf) < tol && logical(eps);

%%
% Test on an array-valued function:
pref.extrapolate = 0;
pref.refinementFunction = 'nested';
f = @(x) [sin(x) cos(x) exp(x)];
g = populate(chebtech2, f, data, pref);
x = chebtech2.chebpts(length(g.coeffs));
values = g.coeffs2vals(g.coeffs);
pass(9) = norm(f(x) - values, inf) < tol;

pref.extrapolate = 1;
pref.refinementFunction = 'nested';
g = populate(chebtech2, f, data, pref);
x = chebtech2.chebpts(length(g.coeffs));
values = g.coeffs2vals(g.coeffs);
pass(10) = norm(f(x) - values, inf) < tol;

pref.extrapolate = 0;
pref.refinementFunction = 'resampling';
g = populate(chebtech2, f, data, pref);
x = chebtech2.chebpts(length(g.coeffs));
values = g.coeffs2vals(g.coeffs);
pass(11) = norm(f(x) - values, inf) < tol;

pref.extrapolate = 1;
pref.refinementFunction = 'resampling';
g = populate(chebtech2, f, data, pref);
x = chebtech2.chebpts(length(g.coeffs));
values = g.coeffs2vals(g.coeffs);
pass(12) = norm(f(x) - values, inf) < tol;

%%
% Some other tests:

% This should fail with an error:
try
    f = @(x) x + NaN;
    populate(chebtech2, f, data, pref);
    pass(13) = false;
catch ME
    pass(13) = strcmp(ME.message, 'Too many NaNs/Infs to handle.');
end

% As should this:
try
    f = @(x) x + Inf;
    populate(chebtech2, f, data, pref);
    pass(14) = false;
catch ME
    pass(14) = strcmp(ME.message, 'Too many NaNs/Infs to handle.');
end

% Test that the extrapolation option avoids endpoint evaluations.
pref.extrapolate = 1;
try
    populate(chebtech2, @(x) [F(x) F(x)], data, pref);
    pass(15) = true;
catch ME %#ok<NASGU>
    pass(15) = false;
end
pref.extrapolate = 0;

    function y = F(x)
        if ( any(abs(x) == 1) )
            error('Extrapolate should prevent endpoint evaluation.');
        end
        y = sin(x);
    end

% Check that things don't crash if pref.minSamples and pref.maxLength are equal.
try
    pref.minSamples = 8;
    pref.maxLength = 8;
    populate(chebtech2, @sin, data, pref);
    pass(16) = true;
catch
    pass(16) = false;
end

%%
% Test logical-valued functions:
f = chebtech2(@(x) x > -2);
g = chebtech2(1);
pass(17) = normest(f - g) < eps;

f = chebtech2(@(x) x < -2);
g = chebtech2(0);
pass(18) = normest(f - g) < eps;

end


classdef IsotropicElasticMaterial < ElasticMaterial

    properties (GetAccess = public, SetAccess = protected)
        nstre
    end

    properties (Access = protected)
        kappa
        mu
        lambda
    end

    methods (Access = public)

        function compute(obj,s)
            obj.kappa = s.kappa;
            obj.mu    = s.mu;
            obj.nElem = size(obj.mu,1);
            obj.nGaus = size(obj.mu,2);
            obj.computeC();
        end

    end

    methods (Access = protected)

        function init(obj,cParams)
            obj.nstre = cParams.nstre;
        end

    end

    methods (Access = protected, Abstract)
        computeC(obj)
    end

    methods (Access = public, Static)

        function mu = computeMuFromYoungAndNu(E,nu)
            mu = E./(2*(1+nu));
        end
    end

end


function boundary=compute_boundary(face, options)

% compute_boundary - compute the vertices on the boundary of a 3D mesh
%
%   boundary=compute_boundary(face);
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;
verb = getoptions(options, 'verb', 1);

if size(face,1)<size(face,2)
    face=face';
end

nvert=max(max(face));
nface=size(face,1);

A=sparse(nvert,nvert);
for i=1:nface
    if verb
        progressbar(i,nface);
    end
    f=face(i,:);
    A(f(1),f(2))=A(f(1),f(2))+1;
    A(f(1),f(3))=A(f(1),f(3))+1;
    A(f(3),f(2))=A(f(3),f(2))+1;
end
A=A+A';

for i=1:nvert
    u=find(A(i,:)==1);
    if ~isempty(u)
        boundary=[i u(1)];
        break;
    end
end

s=boundary(2);
i=2;
while(i<=nvert)
    u=find(A(s,:)==1);
    if length(u)~=2
        warning('problem in boundary');
    end
    if u(1)==boundary(i-1)
        s=u(2);
    else
        s=u(1);
    end
    if s~=boundary(1)
        boundary=[boundary s];
    else
        break;
    end
    i=i+1;
end

if i>nvert
    warning('problem in boundary');
end

%%% OLD %%%
function v = compute_boundary_old(faces)

nvert = max(face(:));
ring = compute_vertex_ring( face );

% compute boundary
v = -1;
for i=1:nvert   % first find a starting vertex
    f = ring{i};
    if f(end)<0
        v = i;
        break;
    end
end
if v<0
    error('No boundary found.');
end
boundary = [v];
prev = -1;
while true
    f = ring{v};
    if f(end)>=0
        error('Problem in boundary');
    end
    if f(1)~=prev
        prev = v;
        v = f(1);
    else
        prev = v;
        v = f(end-1);
    end
    if ~isempty( find(boundary==v) )
        % we have reach the begining of the boundary
        if v~=boundary(1)
            warning('Begining and end of boundary doesn''t match.');
        else
            break;
        end
    end
    boundary = [boundary,v];
end


function A11 = synA11min(A10, A01, cmax)
%-----------------------------------------------------------------------------
%
% For each point of colour 11 this function assigns the minimum value at the
% neighbouring gridpoints of colours 10 and 01.
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: December 7, 2001.
% (c) 1998-2002 Stichting CWI, Amsterdam
%-----------------------------------------------------------------------------
[n10, m10]=size(A10);
[n01, m01]=size(A01);
n11=n10;
m11=m01;
%[n11, m11]=size(A11);
if     m11 == m10
  S=min(stripL(extR(A10, cmax)), A10);
elseif m11 == m10-1
  S=min(stripL(A10), stripR(A10));
else
  disp([' size A10 = ' int2str(size(A10)) ' size A01 = ' int2str(size(A01))]);
  error(' synA11min - A10 and A01 do not match ');
end
if     n11 == n01
  T=min(A01, stripU(extD(A01, cmax)));
elseif n11 == n01-1
  T=min(stripD(A01), stripU(A01));
else
  disp([' size A10 = ' int2str(size(A10)) ' size A01 = ' int2str(size(A01))]);
  error(' synA11min - A10 and A01 do not match ');
end
%Note: all(size(S) == size(T)) & all(size(S) == [n11 m11]) always holds.
A11=min(S, T);
%-----------------------------------------------------------------------------


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
% ##2
%==============================================================================
%
%   - data                 MRI (head), Omega=(0,128)x(0,128), level=4:7, m=[128,128]
%   - viewer               viewImage2D
%   - interpolation        splineInter
%   - distance             MI
%   - pre-registration     rigid2D
%   - regularizer          mbElastic
%   - optimization         lBFGS
% ===============================================================================

close all, help(mfilename);

setup2DMRIData
imgModel('reset','imgModel','splineInter','regularizer','none','theta',1e-3);
distance('reset','distance','MI','nT',8,'nR',8);
trafo('reset','trafo','rigid2D');
regularizer('reset','regularizer','mfElastic','alpha',1e-4,'mu',1,'lambda',0);

PIRpara = optPara('lBFGS','solver','backslash');
NPIRpara = optPara('lBFGS','solver',regularizer('get','solver'));

[yc,wc,his] = MLIR(ML,'PIRobj',@PIRBFGSobjFctn,'PIRpara',PIRpara,...
  'NPIRobj',@NPIRBFGSobjFctn,'NPIRpara',NPIRpara,...
  'minLevel',4,'maxLevel',7,'parametric',1,'plotMLiter',0);

%==============================================================================


function ob = gg_Gnufft(om,M,Msp,R)
%function ob = gg_Gnufft(om, M, Msp, R)
%
% Object constructor for Leslie Greengard's 2-dimensional NUFFT using separable
% gaussian interpolator.
%
% in
%	om [Min,2]	"digital" frequencies in radians
%	M [1]		image dimensions (N1,N2,...,Nd)
%	Msp [1]		# of neighbors used
%	R [1]		oversampling ratio
% out
%	ob              the NUFFT object
%
% A type 1 NUFFT is evaluated using x = G' * b
% A type 2 NUFFT is evaluated using b = G * x
%
% Copyright 2008-6-6	Will Grissom and Jeff Fessler	The University of Michigan

% use greengard-supplied tau
tau = 1/M/M*pi/R/(R-0.5)*Msp; % width of gaussian

% initialize nufft
st = gg_Gnufft_init(om,M,Msp,R,tau);

% build an object with it.
ob = Fatrix([size(om,1) M*M], st, 'forw', @gg_Gnufft_forw, 'back', @gg_Gnufft_back, 'caller', mfilename);


%Evaluate cost function for closest representative search given coefficients
%function ft=essential_distMinAnglePair_ft(t,m1,p1,c1,m2,p2,c2)
%Evaluates the cost function used by essential_distMinAnglePair to find the
%closest representative in the equivalence class of a QREM
%If m2,p2,c2 are omitted or empty, get value of a single term
function ft=essential_distMinAnglePair_ft(t,m1,p1,c1,m2,p2,c2)
flagSingleTerm=false;
if ~exist('m2','var') || isempty(m2)
    flagSingleTerm=true;
end

if flagSingleTerm
    ft=acos((m1*sin(t+p1)+c1-1)/2)^2;
else
    ft=acos((m1*sin(t+p1)+c1-1)/2)^2+acos((m2*sin(t+p2)+c2-1)/2)^2;
end


function [Y,W,SetupStruc] = Process_ILRMA_PF(s,Transfer,SetupStruc)
K = SetupStruc.ILRMA_PF.K;
hop = SetupStruc.ILRMA_PF.hop;
win = hanning(K,'periodic');
win = win/sqrt(sum(win(1:hop:K).^2));
SetupStruc.ILRMA_PF.win = win;  % Preserve 'win' in 'SetupStruc'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N = size(s,2);
for i = 1:N
    X(:,:,i) = fft(enframe(s(:,i),win,hop)');
end
frame_N = size(X,2);
K_m = K/2+1;
Num = size(Transfer,3);
Y = zeros((frame_N-1)*hop+K,Num);
Y_f = zeros(Num,frame_N,K);
%%%%%%%%%%%%%%%%%%%%%%%%%% Obtain processing matrix 'W'
X_sp = zeros(Num,frame_N,K_m);
W_ILRMA = zeros(Num,Num,K_m);
V_sp = zeros(Num,N,K_m);
theta = 10^-6;
for i = 1:K_m
    X_f = permute(X(i,:,:),[3 2 1]);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%% Initialize W by PCA
    [E,D] = PCA(X_f,1,Num);
    V = sqrt(D)\E';
    V_sp(:,:,i) = V;
    X_sp(:,:,i) = V*X_f;
    %%%%%%%%%%%% Adjust amplitude of 'w'
    W_o = eye(Num);
    y_f = W_o*V*X_f;
    W_ILRMA(:,:,i) = W_o;
    Y_f(:,:,i) = y_f;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% ILRMA iterations
epsi = 1e-6;
max_iteration = 300;
L = 2*Num;  %%%%% the number of NMF basis
Z = max(rand(Num,L),epsi);
Z = Z./(ones(Num)*Z);
T = max(rand(K_m,L),epsi);
V = max(rand(L,frame_N),epsi);
R = zeros(K_m,frame_N,Num);
for i = 1:Num
    R(:,:,i) = (ones(K_m,1)*Z(i,:)).*T*V;
end
P = permute(Y_f(:,:,1:K_m),[3,2,1]);
P = abs(P).^2;
% K_sp = sqrt(K_m);
pObj = inf;
bZ = zeros(size(Z));
bT = zeros(size(T));
bV = zeros(size(V));
lamd = zeros(Num,1);
A = zeros(1001,2)-1; %%%% Show the decrease of the value of cost funtion, ILRMA max iterations 1000
for iteration = 1:max_iteration
    %%%%%% NMF with partioning function Z
    for i = 1:Num
        bZ(i,:) = sqrt((T'*(P(:,:,i).*(R(:,:,i).^(-2)))).*V*ones(frame_N,1)./((T'*R(:,:,i).^(-1)).*V*ones(frame_N,1)))';
    end
    Z = max(Z./(ones(Num)*Z),epsi);
    for i = 1:Num
        R(:,:,i) = (ones(K_m,1)*Z(i,:)).*T*V;
    end
    for i = 1:K_m
        P_temp = permute(P(i,:,:),[2 3 1]);
        R_temp = permute(R(i,:,:),[2 3 1]);
        bT(i,:) = sqrt((V*(P_temp.*(R_temp.^(-2)))).*Z'*ones(Num,1)./((V*R_temp.^(-1)).*Z'*ones(Num,1)))';
    end
    T = max(T.*bT,epsi);
    for i = 1:Num
        R(:,:,i) = (ones(K_m,1)*Z(i,:)).*T*V;
    end
    for i = 1:frame_N
        P_temp = permute(P(:,i,:),[1 3 2]);
        R_temp = permute(R(:,i,:),[1 3 2]);
        bV(:,i) = sqrt((T'*(P_temp.*(R_temp.^(-2)))).*Z'*ones(Num,1)./((T'*R_temp.^(-1)).*Z'*ones(Num,1)));
    end
    V = max(V.*bV,epsi);
    for i = 1:Num
        R(:,:,i) = (ones(K_m,1)*Z(i,:)).*T*V;
    end
    %%%%% AuxIVA
    dlw = 0;
    for i = 1:K_m
        W = W_ILRMA(:,:,i);
        X_f = X_sp(:,:,i);
       dlw = dlw +log(abs(det(W))+epsi);
        for i_n = 1:Num
            G_ = permute(R(i,:,i_n),[3 2 1]);
            G_ = repmat(G_+epsi,Num,1);
            Vk = (X_f./G_)*X_f'/frame_N;
            if rcond(Vk)<theta
                Vk = Vk+eye(Num)*min(eig(Vk))*theta;
            end
            wk = inv(W*Vk);
            wk = wk(:,i_n);
            wk = wk/(sqrt(wk'*Vk*wk)+epsi);
            W(i_n,:) = wk';
        end
        W_ILRMA(:,:,i) = W;
        Y_f(:,:,i) = W*X_f;
    end
    P = permute(Y_f(:,:,1:K_m),[3,2,1]);
    P = abs(P).^2;
    Obj = ((sum(sum(sum(log(R+epsi))))+sum(sum(sum(P./R))))/frame_N-2*dlw)/(Num*K_m);
    dObj = pObj-Obj;
    pObj = Obj;
    A(iteration,:) = [Obj,abs(dObj)/abs(Obj)];
    if(abs(dObj)/abs(Obj)<theta)
       break;
    end
    for i = 1:Num
        lamda = sqrt(sum(sum(P(:,:,i))/(frame_N*K_m)));
        lamd(i) = lamda;
        for i_k = 1:K_m
            W_ILRMA(i,:,i_k) = W_ILRMA(i,:,i_k)/lamda;
            for l = 1:L
                T(i_k,l) = T(i_k,l)*sum(Z(:,l))/lamda^2;
            end
        end
        P(:,:,i) = P(:,:,i)/lamda^2;
        R(:,:,i) = R(:,:,i)/lamda^2;
    end
    for l = 1:L
        Z_temp = sum(Z(:,l)./lamd.^(2));
        for i = 1:Num
            Z(i,l) = Z(i,l)/lamd(i)^2/Z_temp;
        end
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% Post processing
W = zeros(Num,N,K_m);
Y_f(:,:,1) = zeros(Num,frame_N);
for i = 2:K_m
    W_inv = pinv(W_ILRMA(:,:,i)*V_sp(:,:,i));
    for ii = 1:Num
        Y_f(ii,:,i) = Y_f(ii,:,i)*W_inv(1,ii);
        W_ILRMA(ii,:,i) = W_ILRMA(ii,:,i)*W_inv(1,ii);
    end
    W(:,:,i) = W_ILRMA(:,:,i)*V_sp(:,:,i);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if(i~=K_m)
        Y_f(:,:,K+2-i) = conj(Y_f(:,:,i));
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Recover signals
if(K/hop==2)
    win = ones(K,1);
end
for i = 1:Num
    y_temp = permute(Y_f(i,:,:),[3 2 1]);
    Y(:,i) = overlapadd(real(ifft(y_temp))',win,hop);
end
return;


function[s,t,E] = sta(data_spk,data_lfp,smp,plt,w,T,D,err)
% Spike Triggered Average
%     Usage: [s,t,E] = sta(data_spk,data_lfp,smp,plt,w,T,D,err)
%
% Inputs
%
% Note that all times have to be consistent. If data_spk
% is in seconds, so must be sig and t. If data_spk is in
% samples, so must sig and t. The default is seconds.
%
% data_spk    - strucuture array of spike times data
%               or NaN padded matrix
% data_lfp    - array of lfp data(samples x trials)
%
% Optional...
% plt 'n'|'r' etc
% width kernel smoothing in s
% T = [-0.1 0.1] - extract this range about each spk
% D = plot spike triggered average out to [D1 D2]
% err = calcluate error bars (bootstrap)
%
% Outputs:
%
% s  spike triggered average
% t  times
% E  bootstrap standard err

if nargin < 3;error('Require spike, lfp and lfp times');end
if isstruct(data_spk)
   [data_spk]=padNaN(data_spk); % create a zero padded data matrix from input structural array
   sz=size(data_spk);
   if sz(1)>sz(2); data_spk=data_spk'; end;% transpose data to get in form compatible with Murray's routine
else
   sz=size(data_spk);
   if sz(1)>sz(2); data_spk=data_spk'; end;% transpose data to get in form compatible with Murray's routine
end
sz=size(data_lfp);
if sz(1)>sz(2); data_lfp=data_lfp'; end;% transpose data to get in form compatible with Murray's routine
verbose = 1;
t = smp;
if nargin < 4; plt = 'r'; end
if nargin < 5; w = 0; end
if nargin < 6; T = [min(t) max(t)]; end
if nargin < 7; D = 0.25*[-1 1]; end
if nargin < 8; err = 1;end

if isempty(plt); plt = 'r'; end
if isempty(w); w = 0; end
if isempty(T); T = [min(t) max(t)]; end
if isempty(D); D = 0.25*[-1 1]; end
if isempty(err); err = 1;end

if w > (T(2)-T(1))/2
  disp('Smoothing > data segment : should be in seconds : turn off smoothing')
  w = 0;
end

sz = size(data_spk);
NT = sz(1);
mlfp = 0;
slfp = 0;
Nspk = 0;
smp = t(2)-t(1);
if D(1) <= 0 && D(2) >= 0
  t1 = [D(1):smp:(-smp+eps) 0:smp:D(2)+eps];
else
  t1 = (round(D(1)/smp)*smp):smp:D(2);
end

% count up the spikes...

if err
  for n=1:NT
    indx = find(t>T(1)&t<T(2));
%     lfp = data_lfp(n,indx);
    spk = data_spk(n,data_spk(n,:)>T(1) && data_spk(n,:)<T(2) && data_spk(n,:)~=0);
    tt = t(indx);
    if ~isempty(spk) > 0
      ND = length(spk);
      for s=1:ND
        spktime = spk(s);
        t0 = tt-spktime + eps;
        if min(t0)>(D(1)-smp) || max(t0)<(D(2)+smp); break;end
        Nspk = Nspk + 1;
      end
    end
  end
  Err = zeros(Nspk,length(t1));
  Nspk = 0;
end

for n=1:NT
  indx = find(t>T(1)&t<T(2));
  lfp = data_lfp(n,indx);
  spk = data_spk(n,data_spk(n,:)>T(1) && data_spk(n,:)<T(2) && data_spk(n,:)~=0);
  tt = t(indx);
  if ~isempty(spk) > 0
    ND = length(spk);
    for s=1:ND
      spktime = spk(s);
      t0 = tt-spktime + eps;
      if min(t0) < (D(1)-smp) && max(t0) > (D(2)+smp);
        indx = find(t0<D(1));
        indx = indx(length(indx));
        offset = (t0(indx)-D(1))/smp;
        indx = indx:(indx+length(t1)-1);
        lfp_t1 = lfp(indx) + (lfp(indx+1)-lfp(indx))*offset;
        Nspk = Nspk + 1;
        mlfp = mlfp + lfp_t1;
        slfp = slfp + lfp_t1.^2;
        Err(Nspk,:) = lfp_t1;
      end
    end
  end
end
if Nspk == 0
  if verbose;disp('No spikes in interval');end
  t = t1;
  s = zeros(length(t),1);
  E = zeros(length(t),1);
  return
end
mlfp = mlfp/Nspk;
slfp = slfp/Nspk;
stdlfp = sqrt((slfp - mlfp.^2)/Nspk);

% local smoother...

N = fix(w/smp);
if N > 5
  mlfp = locsmooth(mlfp,N,fix(N/2));
end

% bootstrap errorbars...

if err == 1;
  Nboot = 20;
  blfp = 0;
  slfp = 0;
  for n = 1:Nboot
    indx = floor(Nspk*rand(1,Nspk)) + 1;
    lfptmp = mean(Err(indx,:));
    if N > 5
      lfptmp = locsmooth(lfptmp,N,fix(N/2));
    end
    blfp = blfp + lfptmp;
    slfp = slfp + lfptmp.^2;
  end
  stdlfp = sqrt((slfp/Nboot - blfp.^2/Nboot^2));
end

s = mlfp-mean(mlfp);
E = stdlfp;
t = t1;

%cols = 'krbgycm';
if plt == 'n';return;end
plot(t1,s,plt)
xax = get(gca,'xlim');
%yax = get(gca,'ylim');
if err == 1
  me = real(2*mean(stdlfp));
  line(xax,me*[1 1],'color','b')
  line(xax,-me*[1 1],'color','b')
  line(xax,0*[1 1],'color','k')
%  errorbar(0.1*xax(2)+0.9*xax(1),0.1*yax(2)+0.9*yax(1), ...
%         mean(stdlfp),'k')
%plot(0.1*xax(2)+0.9*xax(1),0.1*yax(2)+0.9*yax(1),'k.')
end

title(['spike triggered average : ' ...
      num2str(Nspk) ' used : '     ...
      ' Errorbars are two std err']);
%line(get(gca,'xlim'),mean(mlfp)*[1 1],'color','k')
line([0 0],get(gca,'ylim'),'color','k')
hold off


function [ y, m, d, h, n, s, ierror ] = ymdhms_check_common ( y, m, d, h, n, s )

%*****************************************************************************80
%
%% YMDHMS_CHECK_COMMON checks a Common YMDHMS date.
%
%  Discussion:
%
%    The routine will correct certain simple errors in dates, such as
%      "11:03:42 31 September 1996"
%    which will become
%      "11:03:42 1 October 1996".
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, M, D, H, N, S.
%    These items have the obvious meanings.
%    The routine may change any of these values to more reasonable values.
%
%    Output, integer IERROR, is 0 if no error was detected in the
%    date, and 1 otherwise.
%

%
%  Check that the second is between 0 and 59.
%  N may get bumped up or down.
%
  [ y, m, d, h, n, s ] = second_borrow_common ( y, m, d, h, n, s );

  [ y, m, d, h, n, s ] = second_carry_common ( y, m, d, h, n, s );
%
%  Check that the minute is between 0 and 59.
%  H may get bumped up or down.
%
  [ y, m, d, h, n ] = minute_borrow_common ( y, m, d, h, n );

  [ y, m, d, h, n ] = minute_carry_common ( y, m, d, h, n );
%
%  Check that the hour is between 0 and 23.
%  D may get bumped up or down.
%
  [ y, m, d, h ] = hour_borrow_common ( y, m, d, h );

  [ y, m, d, h ] = hour_carry_common ( y, m, d, h );
%
%  Now make adjustments to D, M, and Y.
%
  [ y, m, d, ierror ] = ymd_check_common ( y, m, d );

  return
end


echo on
% This script shows how to use the ga using a float representation.
% You should see the demos for
% more information as well. gademo1, gademo2, gademo3
global bounds

% Setting the seed back to the beginning for comparison sake
rand('seed',0)

% Crossover Operators
xFns = 'simpleXover';
xOpts = [.4];

% Mutation Operators
mFns = 'binaryMutation';

mOpts = [0.005];

% Termination Operators
termFns = 'maxGenTerm';
termOps = [200]; % 200 Generations

% Selection Function
selectFn = 'roulette'
selectOps = [];

% Evaluation Function
evalFn = 'gaMichEval';
evalOps = [];

type gaMichEval

% Bounds on the variables
bounds = [-3 12.1; 4.1 5.8];

% GA Options [epsilon float/binar display]
gaOpts=[1e-6 0 1];

% Generate an intialize population of size 20
startPop = initializega(20,bounds,'gaMichEval',[],[1e-6 0]);

% Lets run the GA
% Hit a return to continue
pause

[x endPop bestPop trace]=ga(bounds,evalFn,evalOps,startPop,gaOpts,...
    termFns,termOps,selectFn,selectOps,xFns,xOpts,mFns,mOpts);

% x is the best solution found
x
% Hit a return to continue
pause

% endPop is the ending population
endPop
% Hit a return to continue
pause

% trace is a trace of the best value and average value of generations
trace
% Hit a return to continue
pause

% Plot the best over time
clf
plot(trace(:,1),trace(:,2));
% Hit a return to continue
pause

% Add the average to the graph
hold on
plot(trace(:,1),trace(:,3));
% Hit a return to continue
pause

% Lets increase the population size by running the defaults
%
rand('seed',0)
termOps=[100];
[x endPop bestPop trace]=ga(bounds,evalFn,evalOps,[],gaOpts,termFns,termOps,...
    selectFn,selectOps);

% x is the best solution found
x
% Hit a return to continue
pause

% endPop is the ending population
endPop
% Hit a return to continue
pause

% trace is a trace of the best value and average value of generations
trace
% Hit a return to continue
pause

% Plot the best over time
clf
plot(trace(:,1),trace(:,2));
% Hit a return to continue
pause

% Add the average to the graph
hold on
plot(trace(:,1),trace(:,3));

echo off


function [P,Uinit,output] = cp_als(X,R,varargin)
%CP_ALS Compute a CP decomposition of any type of tensor.
%
%   P = CP_ALS(X,R) computes an estimate of the best rank-R
%   CP model of a tensor X using an alternating least-squares
%   algorithm.  The input X can be a tensor, sptensor, ktensor, or
%   ttensor. The result P is a ktensor.
%
%   P = CP_ALS(X,R,'param',value,...) specifies optional parameters and
%   values. Valid parameters and their default values are:
%      'tol' - Tolerance on difference in fit {1.0e-4}
%      'maxiters' - Maximum number of iterations {50}
%      'dimorder' - Order to loop through dimensions {1:ndims(A)}
%      'init' - Initial guess [{'random'}|'nvecs'|cell array]
%      'printitn' - Print fit every n iterations; 0 for no printing {1}
%
%   [P,U0] = CP_ALS(...) also returns the initial guess.
%
%   [P,U0,out] = CP_ALS(...) also returns additional output that contains
%   the input parameters.
%
%   Note: The "fit" is defined as 1 - norm(X-full(P))/norm(X) and is
%   loosely the proportion of the data described by the CP model, i.e., a
%   fit of 1 is perfect.
%
%   NOTE: Updated in various minor ways per work of Phan Anh Huy. See Anh
%   Huy Phan, Petr Tichavsk? Andrzej Cichocki, On Fast Computation of
%   Gradients for CANDECOMP/PARAFAC Algorithms, arXiv:1204.1586, 2012.
%
%   Examples:
%   X = sptenrand([5 4 3], 10);
%   P = cp_als(X,2);
%   P = cp_als(X,2,'dimorder',[3 2 1]);
%   P = cp_als(X,2,'dimorder',[3 2 1],'init','nvecs');
%   U0 = {rand(5,2),rand(4,2),[]}; %<-- Initial guess for factors of P
%   [P,U0,out] = cp_als(X,2,'dimorder',[3 2 1],'init',U0);
%   P = cp_als(X,2,out.params); %<-- Same params as previous run
%
%   See also KTENSOR, TENSOR, SPTENSOR, TTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Extract number of dimensions and norm of X.
N = ndims(X);
normX = norm(X);

%% Set algorithm parameters from input or by using defaults
params = inputParser;
params.addParamValue('tol',1e-4,@isscalar);
params.addParamValue('maxiters',50,@(x) isscalar(x) & x > 0);
params.addParamValue('dimorder',1:N,@(x) isequal(sort(x),1:N));
params.addParamValue('init', 'random', @(x) (iscell(x) || ismember(x,{'random','nvecs'})));
params.addParamValue('printitn',1,@isscalar);
params.parse(varargin{:});

%% Copy from params object
fitchangetol = params.Results.tol;
maxiters = params.Results.maxiters;
dimorder = params.Results.dimorder;
init = params.Results.init;
printitn = params.Results.printitn;

%% Error checking

%% Set up and error checking on initial guess for U.
if iscell(init)
    Uinit = init;
    if numel(Uinit) ~= N
        error('OPTS.init does not have %d cells',N);
    end
    for n = dimorder(2:end);
        if ~isequal(size(Uinit{n}),[size(X,n) R])
            error('OPTS.init{%d} is the wrong size',n);
        end
    end
else
    % Observe that we don't need to calculate an initial guess for the
    % first index in dimorder because that will be solved for in the first
    % inner iteration.
    if strcmp(init,'random')
        Uinit = cell(N,1);
        for n = dimorder(2:end)
            Uinit{n} = rand(size(X,n),R);
        end
    elseif strcmp(init,'nvecs') || strcmp(init,'eigs')
        Uinit = cell(N,1);
        for n = dimorder(2:end)
            Uinit{n} = nvecs(X,n,R);
        end
    else
        error('The selected initialization method is not supported');
    end
end

%% Set up for iterations - initializing U and the fit.
U = Uinit;
fit = 0;

if printitn>0
  fprintf('\nCP_ALS:\n');
end

%% Main Loop: Iterate until convergence

if (isa(X,'sptensor') || isa(X,'tensor')) && (exist('cpals_core','file') == 3)

    %fprintf('Using C++ code\n');
    [lambda,U] = cpals_core(X, Uinit, fitchangetol, maxiters, dimorder);
    P = ktensor(lambda,U);

else

    UtU = zeros(R,R,N);
    for n = 1:N
        if ~isempty(U{n})
            UtU(:,:,n) = U{n}'*U{n};
        end
    end

    for iter = 1:maxiters

        fitold = fit;

        % Iterate over all N modes of the tensor
        for n = dimorder(1:end)

            % Calculate Unew = X_(n) * khatrirao(all U except n, 'r').
            Unew = mttkrp(X,U,n);

            % Compute the matrix of coefficients for linear system
            Y = prod(UtU(:,:,[1:n-1 n+1:N]),3);
            Unew = Unew / Y;
            if issparse(Unew)
                Unew = full(Unew);   % for the case R=1
            end

            % Normalize each vector to prevent singularities in coefmatrix
            if iter == 1
                lambda = sqrt(sum(Unew.^2,1))'; %2-norm
            else
                lambda = max( max(abs(Unew),[],1), 1 )'; %max-norm
            end

            Unew = bsxfun(@rdivide, Unew, lambda');

            U{n} = Unew;
            UtU(:,:,n) = U{n}'*U{n};
        end

        P = ktensor(lambda,U);
        if normX == 0
            fit = norm(P)^2 - 2 * innerprod(X,P);
        else
            normresidual = sqrt( normX^2 + norm(P)^2 - 2 * innerprod(X,P) );
            fit = 1 - (normresidual / normX); %fraction explained by model
        end
        fitchange = abs(fitold - fit);

        % Check for convergence
        if (iter > 1) && (fitchange < fitchangetol)
            flag = 0;
        else
            flag = 1;
        end

        if (mod(iter,printitn)==0) || ((printitn>0) && (flag==0))
            fprintf(' Iter %2d: f = %e f-delta = %7.1e\n', iter, fit, fitchange);
        end

        % Check for convergence
        if (flag == 0)
            break;
        end
    end
end

%% Clean up final result
% Arrange the final tensor so that the columns are normalized.
P = arrange(P);
% Fix the signs
P = fixsigns(P);

if printitn>0
    if normX == 0
        fit = norm(P)^2 - 2 * innerprod(X,P);
    else
        normresidual = sqrt( normX^2 + norm(P)^2 - 2 * innerprod(X,P) );
        fit = 1 - (normresidual / normX); %fraction explained by model
    end
  fprintf(' Final f = %e \n', fit);
end

output = struct;
output.params = params.Results;
output.iters = iter;


% MODULUS_LAYER Calculate the modulus of coefficients in a layer
%
% Usage
%    U = MODULUS_LAYER(W)
%
% Input
%    W (struct): A scattering layer, as output by WAVELET_LAYER_*, for exam-
%       ple.
%
% Output
%    U (struct): The same layer with all coefficients set to their absolute
%       values.
%
% See Also
%    PAD_SIGNAL, UNPAD_SIGNAL

function U = modulus_layer(W)
	U.signal = cellfun(@abs, W.signal, 'UniformOutput', 0);
	U.meta = W.meta;
end


function [PB] = bit2PB(bit)
% Convert computery things from bits to petabytes.
% Chad A. Greene 2012
PB = bit*2^-53;


function sF = approximation(nodes, y, varargin)
% computes a least square problem to get an approximation
% Syntax
%   sF = S2FunHarmonic.approximation(S2Grid, f)
%   sF = S2FunHarmonic.approximation(S2Grid, f, 'bandwidth', bandwidth, 'tol', TOL, 'maxit', MAXIT, 'weights', W)
%
% Input
%  S2Grid - grid on the sphere
%  f      - function values on the grid (may be multidimensional)
%
% Options
%  bandwidth  - maximum degree of the spherical harmonics used to approximate the function
%  to         - tolerance for lsqm
%  maxIt      - maximum number of iterations for lsqm
%  W          - weight w_n for the node nodes (default: voronoi weights)
%

% make points unique
s = size(y);
y = reshape(y, length(nodes), []);
[nodes,~,ind] = unique(nodes(:));

% take the mean over duplicated nodes
for k = 1:size(y,2)
  yy(:,k) = accumarray(ind,y(:,k),[],@nanmean); %#ok<AGROW>
end

% consider the case of symmetry
if isa(nodes,'Miller')

  nodes = nodes.symmetrise('noAntipodal').';
  nodes.antipodal = nodes.CS.isLaue;
  yy = repmat(yy,size(nodes,2),1);
  nodes = nodes(:);

end

y = reshape(yy, [length(nodes) s(2:end)]);

tol = get_option(varargin, 'tol', 1e-6);
maxit = get_option(varargin, 'maxit', 40);

if check_option(varargin, 'antipodal') || nodes.antipodal
  nodes.antipodal = true;
  bw = get_option(varargin, 'bandwidth', ceil(sqrt(length(nodes))));
  bw = floor(bw/2)*2; % make bandwidth even
  mask = sparse((bw+1)^2); % only use even polynomial degree
  for m = 0:2:bw
    mask((m^2+1):(m^2+2*m+1), (m^2+1):(m^2+2*m+1)) = speye(2*m+1);
  end
else
  bw = get_option(varargin, 'bandwidth', ceil(sqrt(length(nodes)/2)));
  mask = speye((bw+1)^2);
end

W = get_option(varargin, 'weights');
if isempty(W)
  W = nodes.calcVoronoiArea;
else
  W = W(ind);
end
W = sqrt(W(:));

% initialize nfsft
nfsftmex('precompute', bw, 1000, 1, 0);
plan = nfsftmex('init_advanced', bw, length(nodes), 1);
nfsftmex('set_x', plan, [nodes.rho'; nodes.theta']); % set vertices
nfsftmex('precompute_x', plan);

b = W.*y;

s = size(b);
b = reshape(b, s(1), []);
num = size(b, 2);

fhat = zeros((bw+1)^2, num);
for index = 1:num
  [fhat(:, index), flag] = lsqr(...
    @(x, transp_flag) afun(transp_flag, x, plan, W, mask), ...
    b(:, index), tol, maxit);
  fhat(:, index) = mask*fhat(:, index);
end
fhat = reshape(fhat, [(bw+1)^2 s(2:end)]);

% finalize nfsft
nfsftmex('finalize', plan);

sF = S2FunHarmonic(fhat);

% ensure symmetry if required
if isa(nodes,'Miller'), sF = sF.symmetrise(nodes.CS); end

end

function y = afun(transp_flag, x, plan, W, mask)

if strcmp(transp_flag, 'transp')

  x = x.*W;

  nfsftmex('set_f', plan, x);
  nfsftmex('adjoint', plan);
  y = nfsftmex('get_f_hat_linear', plan);

  y = mask*y;

elseif strcmp(transp_flag, 'notransp')

  x = mask*x;

  nfsftmex('set_f_hat_linear', plan, x);
  nfsftmex('trafo', plan);
  y = nfsftmex('get_f', plan);

  y = y.*W;

end

end


%  Figure 7.51      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% script for fig. 7.51
%
clf;
dp=[1 1 0];
np=[1];
nc=conv([1 1.001],[8.32 0.8]);
dc=conv([1 4.08],[1 0.0196]);
num=conv(np,nc);
den=conv(dp,dc);
sys=tf(num,den);
rlocus(sys);


%{

This code is to render a Mesh given a 3x4 camera matrix with an image resolution widthxheight. The rendering result is an ID map for facets, edges and vertices. This can usually used for occlusion testing in texture mapping a model from an image, such as the texture mapping in the following two papers.

--Jianxiong Xiao http://mit.edu/jxiao/

Citation:

[1] J. Xiao, T. Fang, P. Zhao, M. Lhuillier, and L. Quan
Image-based Street-side City Modeling
ACM Transaction on Graphics (TOG), Volume 28, Number 5
Proceedings of ACM SIGGRAPH Asia 2009

[2] J. Xiao, T. Fang, P. Tan, P. Zhao, E. Ofek, and L. Quan
Image-based Facade Modeling
ACM Transaction on Graphics (TOG), Volume 27, Number 5
Proceedings of ACM SIGGRAPH Asia 2008

%}

clear
clc
close all

compile
load('data.mat')

result = RenderMex(P, img_width, img_height, vertex, edge, face)';

close all
imagesc(result)
axis equal
axis tight
max(max(result))


%SVC Trainable classifier: Support Vector Machine
%
% 	[W,J] = SVC(A,KERNEL,C)
%   [W,J] = A*SVC([],KERNEL,C)
%   [W,J] = A*SVC(KERNEL,C)
%
% INPUT
%   A	      Dataset
%   KERNEL  - Untrained mapping to compute kernel by A*(A*KERNEL) during
%             training, or B*(A*KERNEL) during testing with dataset B.
%           - String to compute kernel matrices by FEVAL(KERNEL,B,A)
%           Default: linear kernel (PROXM('p',1))
%   C       Regularization parameter (optional; default: 1)
%
% OUTPUT
%   W       Mapping: Support Vector Classifier
%   J       Object indices of support objects
%
% DESCRIPTION
% Optimizes a support vector classifier for the dataset A by quadratic
% programming. The non-linearity is determined by the kernel.
% If KERNEL = 0 it is assumed that A is already the kernelmatrix (square).
% In this case also a kernel matrix B should be supplied at evaluation by
% B*W or PRMAP(B,W).
%
% There are several ways to define KERNEL, e.g. PROXM('r',1) for a
% radial basis kernel or by USERKERNEL for a user defined kernel.
%
% If C is NaN this regularisation parameter is optimised by REGOPTC.
%
% See for more possibilties SVCINFO
%
% EXAMPLE
% a = gendatb;                     % generate banana classes
% [w,J] = a*svc(proxm('p',3));  % compute svm with 3rd order polynomial
% a*w*testc                        % show error on train set
% scatterd(a)                      % show scatterplot
% plotc(w)                         % plot classifier
% hold on;
% scatterd(a(J,:),'o')             % show support objcts
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, PROXM, USERKERNEL, NUSVC, RBSVC, LIBSVC, REGOPTC

% Copyright: D. de Ridder, D. Tax, S. Verzakov, R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function [W,J,C,nu,alginf] = svc(varargin)

  mapname = 'SVC';
  argin = shiftargin(varargin,{'prmapping','char','cell'});
  argin = setdefaults(argin,[],proxm([],'p',1),1,1);

  if mapping_task(argin,'definition')

    W = define_mapping(argin,'untrained',mapname);

	elseif mapping_task(argin,'training')			% Train a mapping.

    [a,kernel,C,Options] = check_for_old_call(argin);
    if isequal(Options,1), Options = []; end

    DefOptions.mean_centering   = 1;
    DefOptions.pd_check         = 1;
    DefOptions.bias_in_admreg   = 1;
    DefOptions.pf_on_failure    = 1;
    DefOptions.multiclass_mode  = 'single';

    Options = updstruct(DefOptions,Options,1);

    islabtype(a,'crisp');
    isvaldfile(a,1,2); % at least 1 object per class, 2 classes
    a = testdatasize(a,'objects');
    [m,k,c] = getsize(a);
    nlab = getnlab(a);

    % The SVC is basically a 2-class classifier. More classes are
    % handled by mclassc.

    if c == 2   % two-class classifier

      if (isnan(C))|length(C)>1     % optimize trade-off parameter
        defs = {proxm([],'p',1),1,[]};
        if isnan(C) % use a default range of C values
          parmin_max = [0,0;1e-2,1e2;0,0];   % kernel and Options can not be optimised
        else % use the range that is supplied by the user
          parmin_max = [0 0; min(C) max(C); 0 0];
          C = NaN;
        end
        [W,J,C,nu,alginf] = regoptc(a,mfilename,{kernel,C,Options},defs,[2],parmin_max,testc([],'soft'));
      else
        % Compute the parameters for the optimization:
        y = 3 - 2*nlab;
        if isequal(kernel,0)
          s = [];
          u = [];
          in_size = 0; % to allow old and new style calls
        else
          if Options.mean_centering
            u = mean(a);         % shift origin for better accuracy
            a = a - repmat(u,[m,1]);
          else
            u = [];
          end
          in_size = k;
        end
        K = compute_kernel(a,a,kernel);
        K = min(K,K');   % make sure kernel is symmetric
        [v,J,C,nu] = svo(+K,y,C,Options);

        % Store the results:
        if ~isequal(kernel,0)
          s = a(J,:);
        end
        W = prmapping(mfilename,'trained',{u,s,v,kernel,J},getlablist(a),in_size,2);
        % Note: even in case kernel is a mapping, we store the untrained
        % mapping, as training is just administration
        W = cnormc(W,a);
        W = setcost(W,a);

        alginf.svc_type = 'C-SVM';
        alginf.kernel = kernel;
        alginf.C   = C;
        alginf.nu  = nu;
        alginf.nSV = length(J);
        alginf.classsizes = [nnz(y==1), nnz(y==-1)];
        alginf.pf = isnan(nu);
        W = setname(W,'SVM');

      end

    else   % multi-class classifier:

      [W,J,C,nu,alginf] = mclassc(a,prmapping(mfilename,{kernel,C,Options}),Options.multiclass_mode);
      W = W*classc;

    end
    W = setname(W,mapname);

  else % Evaluation

    [a,v] = deal(argin{1:2});
    w = +v;
    m = size(a,1);

    if isempty(w{2})
      K = a; % user supplied testkernel
      J = w{5};
      if size(K,2) > length(J) & size(K,2) >= max(J)
        K = K(:,J); % probably full test kernel
      elseif size(K,2) ~= length(J)
        error('Wrong test kernel supplied')
      end
    else
      % The first parameter w{1} stores the mean of the dataset. When it
      % is supplied, remove it from the dataset to improve the numerical
      % precision. Then compute the kernel matrix using proxm:
      if isempty(w{1})
        K = a*(w{2}*w{4});
      else
        K = (a-ones(m,1)*w{1})*(w{2}*w{4});
      end
    end
    % Data is mapped by the kernel, now we just have a linear
    % classifier  w*x+b:
    d = [K ones(m,1)] * w{3};
    W = setdat(a,[d -d],v);

  end

return

function K = compute_kernel(a,s,kernel)

	% compute a kernel matrix for the objects a w.r.t. the support objects s
	% given a kernel description

	if  ischar(kernel) % routine supplied to compute kernel
		K = feval(kernel,a,s);
	elseif iscell(kernel)
		K = feval(kernel{1},a,s,kernel{2:end});
	elseif ismapping(kernel)
		K = a*prmap(s,kernel);
	elseif kernel == 0 % we have already a kernel
		K = a;
	else
		error('Do not know how to compute kernel matrix')
	end

	K = +K;

return

function [a,kernel,C,par] = check_for_old_call(argin)

  [a,kernel,C,par] = deal(argin{:});
  if ischar(kernel) && exist(kernel,'file') ~= 2
    kernel = proxm(kernel,C);
    C = par;
    par = [];
  end

return


function [x,w,P,Z] = fnnlsb(XtX,Xty,P_old,Z_old,tol)
%FNNLSb	Non-negative least-squares.
%
%     	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%	%							 %
%	% Please note this version of NNLS is for advanced users %
%	% Please refer to the m-file FNNLS for a simpler fast    %
%	% version of NNLS				         %
%	%						         %
%     	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
% 	Adapted from NNLS of Mathworks, Inc.
%
%	x = fnnlsb(A,b) returns the vector X that solves x = pinv(A)*b
%	in a least squares sense, subject to x >= 0.
%
%	A default tolerance of TOL = MAX(SIZE(A)) * NORM(A,1) * EPS
%	is used for deciding when elements of x are less than zero.
%	This can be overridden with x = fnnlsb(A,b,P_old,Z_old,TOL).
%
%	[x,w] = fnnlsb(A,b) also returns dual vector w where
%	w(i) < 0 where x(i) = 0 and w(i) = 0 where x(i) > 0.
%       [X,W,P,Z] = fnnlsb(A,b) also returns the index set P and Z
%	that explicitly tells which constraints (variables) are
% 	active, which are not. These can be used as subsequent
%	input to fnnlsb if only minor changes are expected in a new
%	problem. P designates the passive (unconstrained) set, while
%	designates the active constraints. For initialisation use
% 	P=zeros(size(Xty));
%	Z=[1:length(Xty)]';
%
%	See also FNNLS and NNLS

%	L. Shure 5-8-87
%	Revised, 12-15-88,8-31-89 LS.
%	(Partly) Copyright (c) 1984-94 by The MathWorks, Inc.

%	Modified by R. Bro 5-7-96 according to
%       Bro R., de Jong S., Journal of Chemometrics, 1997, 11, 393-401
% 	Corresponds to the FNNLSb algorithm in the paper
%
%
%	Rasmus bro
%	Chemometrics Group, Food Technology
%	Dept. Dairy and Food Science
%	Royal Vet. & Agricultural
%	DK-1958 Frederiksberg C
%	Denmark
%	rb@kvl.dk
%	http://newton.foodsci.kvl.dk/rasmus.html

%  Reference:
%  Lawson and Hanson, "Solving Least Squares Problems", Prentice-Hall, 1974.

% initialize variables
if nargin < 5
    tol = 10*eps*norm(XtX,1)*max(size(XtX));
end
[m,n] = size(XtX);
P = P_old(:)';
PP = find(P);
Z = Z_old(:)';
ZZ=find(Z);
iter = 0;
itmax = 30*n;
z=zeros(n,1);
x=z;
z(PP)=(Xty(PP)'/XtX(PP,PP)');

    while any((z(PP) <= tol)) & iter < itmax
        iter = iter + 1;
        QQ = find((z <= tol) & P');
        alpha = min(x(QQ)./(x(QQ) - z(QQ)));
        x = x + alpha*(z - x);
        ij = find(abs(x) < tol & P' ~= 0);
        Z(ij)=ij';
        P(ij)=zeros(1,max(size(ij)));
        PP = find(P);
        ZZ = find(Z);
        nzz = size(ZZ);
        z(PP)=(Xty(PP)'/XtX(PP,PP)');
        z(ZZ) = zeros(nzz(2),nzz(1));
        z=z(:);
    end
    x = z;
    w = Xty-XtX*x;

% set up iteration criterion
iter = 0;

% outer loop to put variables into set to hold positive coefficients
while any(Z) & any(w(ZZ) > tol)
    [wt,t] = max(w(ZZ));
    t = ZZ(t);
    P(1,t) = t;
    Z(t) = 0;
    PP = find(P);
    ZZ = find(Z);
    nzz = size(ZZ);
    z(PP)=(Xty(PP)'/XtX(PP,PP)');
    z(ZZ) = zeros(nzz(2),nzz(1))';
    z=z(:);
    while any((z(PP) <= tol)) & iter < itmax
        iter = iter + 1;
        QQ = find((z <= tol) & P');
        alpha = min(x(QQ)./(x(QQ) - z(QQ)));
        x = x + alpha*(z - x);
        ij = find(abs(x) < tol & P' ~= 0);
        Z(ij)=ij';
        P(ij)=zeros(1,max(size(ij)));
        PP = find(P);
        ZZ = find(Z);
        nzz = size(ZZ);
        z(PP)=(Xty(PP)'/XtX(PP,PP)');
        z(ZZ) = zeros(nzz(2),nzz(1));
        z=z(:);
    end
    x = z;
    w = Xty-XtX*x;
end

x=x(:);


function borderCoordsRoi=dtiFindBorderBetweenRois(roi1_img, roi2_img, dt6File, minDist, borderRoiName)
%Find a shared border separating two ROIs
%
% borderCoords=dtiFindBorderBetweenRois(roi1_img, roi2_img, dt6File, [minDist=1.87])
%
% Given two nifti images with ROIs, find a set of points where the two
% ROIs are touching within the distance set by minDist parameter.
%
% Example:
%        dt6File=fullfile(pathtoChild, 'dt6.mat');
%        roi1_img=fullfile(pathtoChildInFreesurfer, 'aparc_aseg1015.nii');
%        roi2_img=fullfile(pathtoChildInFreesurfer, 'aparc_aseg1030.nii');
%        borderCoordsRoi=dtiFindBorderBetweenRois(roi1_img, roi2_img, dt6File, 5, borderRoiName);
%        borderCoordsRoiFile=fullfile(pathtoChild, 'ROIs', 'border1015to1030');
%        borderRoiName='borderRoi1toRoi2';
%        dtiWriteRoi(borderCoordsRoi, borderCoordsRoiFile);
%
% (c) Vistalab

% HISTORY:
% 03/2010 ER wrote it

if ~exist('minDist', 'var') || isempty(minDist)
minDist=1.87; %mm
end

roi1=dtiImportRoiFromNifti(roi1_img, dt6File);
roi2=dtiImportRoiFromNifti(roi2_img, dt6File);

[indices, bestSq]=nearpoints(roi1.coords', roi2.coords');
borderCoords=(roi1.coords(bestSq<(minDist^2), :)+roi2.coords(indices(bestSq<(minDist^2)), :))./2;
borderCoordsRoi=dtiNewRoi(borderRoiName, [], borderCoords);


classdef prtClassOneClassRvm < prtClass
    %prtClassOneClassRvm < prtClass
    % This is a one-class RVM.  It learns a regression RVM to approximate
    % an estimated PDF.  You can set the base RV used to estimate the PDF
    % with the field baseRv.  By default, it's a prtRvKde.
    %
    % ds = prtDataGenUnimodal;
    % ocRvm = prtClassOneClassRvm;
    % ocRvm = train(ocRvm,ds);
    % plot(ocRvm);
    %
    %

    properties (SetAccess=private)
        name = 'One Class RVM'  % Relevance Vector Machine
        nameAbbreviation = 'ocRVM'           % RVM
        isNativeMary = false;  % False
    end
    properties (Dependent)
        kernels
    end

    properties
        useLogPdf = true;
        internalRegressRvm = prtRegressRvm;
        baseRv = prtRvKde;
    end

    methods
        function self = prtClassOneClassRvm(varargin)
            self = prtUtilAssignStringValuePairs(self,varargin{:});
        end

        function self = set.kernels(self,kernelVals)
            self.internalRegressRvm.kernels = kernelVals;
        end
        function outKernels = get.kernels(self)
            outKernels = self.internalRegressRvm.kernels;
        end
    end

    methods (Access=protected, Hidden = true)

        function self = trainAction(self,dataSet)
            %self = trainAction(self,dataSet)

            dsH1 = dataSet.retainClassesByInd(2);

            rv = self.baseRv;
            rv = rv.mle(dsH1);

            if self.useLogPdf
                dsH1Regress = prtDataSetRegress(dsH1.getX,rv.logPdf(dsH1));
            else
                dsH1Regress = prtDataSetRegress(dsH1.getX,rv.pdf(dsH1));
            end

            self.internalRegressRvm = self.internalRegressRvm.train(dsH1Regress);
        end

        function yOut = runAction(self,dataSet)
            yOut = dataSet;

            regressDataSet = prtDataSetRegress(dataSet.getX);
            regressDataSetOut = self.internalRegressRvm.run(regressDataSet);

            if self.useLogPdf
                yOut.X = exp(regressDataSetOut.X);
            else
                yOut.X = regressDataSetOut.X;
            end

        end
    end
end


function Retinex = retinex_mccann99(L, nIterations)

global OPE RRE Maximum
[nrows ncols] = size(L) ;                              % get size of the input image
nLayers = ComputeLayers(nrows, ncols) ;                % compute the number of pyramid layers
nrows = nrows/( 2 ^nLayers) ;                            % size of image to process for layer 0
ncols = ncols/( 2 ^nLayers) ;
if (nrows*ncols > 25 )                                % not processing images of area > 25
  error( ' invalid image size. ' )                       % at first layer
end
Maximum = max(L(:)) ;                                  % maximum color value in the image
OP = Maximum*ones([nrows ncols]) ;                     % initialize Old Product
for layer = 0 :nLayers
   RR = ImageDownResolution(L, 2 ^(nLayers-layer)) ;    % reduce input to required layer size

   OPE = [zeros(nrows, 1 ) OP zeros(nrows, 1 )] ;          % pad OP with additional columns
   OPE = [zeros( 1 ,ncols+ 2 ) ; OPE; zeros(1,ncols+2)];  % and rows
   RRE = [RR(:, 1 ) RR RR(:,end)] ;                      % pad RR with additional columns
   RRE = [RRE( 1 ,:) ; RRE; RRE(end,:)];                % and rows

   for iter = 1 :nIterations
     CompareWithNeighbor(- 1 , 0 ) ;                      % North
     CompareWithNeighbor(- 1 , 1 ) ;                      % North-East
     CompareWithNeighbor( 0 , 1 ) ;                       % East
     CompareWithNeighbor( 1 , 1 ) ;                       % South-East
     CompareWithNeighbor( 1 , 0 ) ;                       % South
     CompareWithNeighbor( 1 , - 1 ) ;                      % South-West
     CompareWithNeighbor( 0 , - 1 ) ;                      % West
     CompareWithNeighbor(- 1 , - 1 ) ;                     % North-West
   end
   NP = OPE( 2 :(end- 1 ), 2 :(end- 1 )) ;
   OP = NP(:, [fix( 1 : 0 . 5 :ncols) ncols]) ;              %%% these two lines are equivalent with
   OP = OP([fix( 1 : 0 . 5 :nrows) nrows], :) ;              %%% OP = imresize(NP, 2) if using Image
   nrows = 2 *nrows ; ncols = 2*ncols;                 % Processing Toolbox in MATLAB
end
Retinex = NP ;

function CompareWithNeighbor(dif_row, dif_col)
global OPE RRE Maximum
% Ratio-Product operation
IP = OPE( 2 + dif_row: (end- 1 +dif_row), 2 + dif_col: (end- 1 +dif_col)) + ...
     RRE( 2 :(end- 1 ), 2 :(end- 1 )) - RRE( 2 + dif_row: (end- 1 +dif_row), 2 + dif_col: (end- 1 +dif_col)) ;

IP(IP > Maximum) = Maximum ;                           % The Reset step

% ignore the results obtained in the rows or columns for which the neighbors are undefined
if (dif_col == - 1 ) IP(:, 1 ) = OPE( 2 :(end- 1 ), 2 ) ; end
if (dif_col == + 1 ) IP(:,end) = OPE( 2 :(end- 1 ),end- 1 ) ; end
if (dif_row == - 1 ) IP( 1 ,:) = OPE( 2 , 2 :(end- 1 )) ; end
if (dif_row == + 1 ) IP(end,:) = OPE(end- 1 , 2 :(end- 1 )) ; end
NP = (OPE( 2 :(end- 1 ), 2 :(end- 1 )) + IP)/ 2 ;               % The Averaging operation
OPE( 2 :(end- 1 ), 2 :(end- 1 )) = NP ;

function Layers = ComputeLayers(nrows, ncols)
power = 2 ^fix(log2(gcd(nrows, ncols))) ;               % start from the Greatest Common Divisor
while(power > 1 & ((rem(nrows, power) ~= 0 ) | (rem(ncols, power) ~= 0 )))
   power = power/ 2 ;                                   % and find the greatest common divisor
end                                                  % that is a power of 2
Layers = log2(power) ;

function Result = ImageDownResolution(A, blocksize)
[rows, cols] = size(A) ;                               % the input matrix A is viewed as
result_rows = rows/blocksize ;                         % a series of square blocks
result_cols = cols/blocksize ;                         % of size = blocksize
Result = zeros([result_rows result_cols]) ;
for crt_row = 1 :result_rows                          % then each pixel is computed as
   for crt_col = 1 :result_cols                       % the average of each such block
      Result(crt_row, crt_col) = mean2(A( 1 +(crt_row- 1 )* blocksize: crt_row*blocksize, ...
                                       1 +(crt_col- 1 )* blocksize: crt_col*blocksize)) ;
   end
en


function gf=ref_wfac(g,a,M)
%REF_WFAC  Compute window factorization
%  Usage: gf=ref_wfac(g,a,M);

% The commented _nos code in this file can be used to test
% the _nos versions of the C-library.

L=size(g,1);
R=size(g,2);

N=L/a;
b=L/M;

c=gcd(a,M);
p=a/c;
q=M/c;
d=N/q;

gf=zeros(p,q*R,c,d);
gf_nos=zeros(d,p,q*R,c);

for w=0:R-1
  for s=0:d-1
    for l=0:q-1
      for k=0:p-1
	gf(k+1,l+1+q*w,:,s+1)=g((1:c)+c*mod(k*q-l*p+s*p*q,d*p*q),w+1);
	%gf_nos(s+1,k+1,l+1+q*w,:)=g((1:c)+c*mod(k*q-l*p+s*p*q,d*p*q),w+1);
      end;
    end;
  end;
end;

% dft them
if d>1
  gf=fft(gf,[],4);
  %gf_nos=fft(gf_nos);
end;

% Scale by the sqrt(M) comming from Walnuts representation
gf=gf*sqrt(M);
%gf_nos=gf_nos*sqrt(M);

%gf_nos=permute(gf_nos,[2, 3, 4, 1]);

%norm(gf_nos(:)-gf(:))

gf=reshape(gf,p*q*R,c*d);


% clear,clc
%
% close all
global bound rng
l1=1;l2=1;l3=0.5;

hh=findobj(gcf,'tag','x1');
xs=str2num(get(hh,'string'));
hh=findobj(gcf,'tag','y1');
ys=str2num(get(hh,'string'));
hh=findobj(gcf,'tag','phi');
phis=str2num(get(hh,'string'));
hh=findobj(gcf,'tag','x2');
xf=str2num(get(hh,'string'));
hh=findobj(gcf,'tag','y2');
yf=str2num(get(hh,'string'));

phi=phis*pi/180;
[k,confi]=invkini(xs,ys,phi);
if k > 1
    warndlg('non valid initial conditions..... change the initial conditions','!! error !!')
    clear
    return
end
if sqrt(xf^2+yf^2)==2.5
   warndlg('there is no redundancy..... change final position','!! error !!')
   clear
   return
end
para=[confi xf yf];
%GA parameters
pops=40;
crossprop=0.8;
mutprop=0.05;
maxgen=80;
bound=[ -pi     pi;% qm1
        -pi     pi;% qm2
        -pi     pi;% qm3
        -pi     pi;% phif
       -pi/4   pi/4;% vqm1
       -pi/4   pi/4;% vqm2
       -pi/4   pi/4;% vqm3
        0       8;% t1
        0       8];%t2

%initialization
numvar=size(bound,1);
rng=(bound(:,2)-bound(:,1))';
pop=zeros(pops,numvar);
%pop = initial population
pop(:,1:numvar)=(ones(pops,1)*rng).*(rand(pops,numvar))+...
    (ones(pops,1)*bound(:,1)');
tic
for it=1:maxgen
    [fpop,z]=fitnesstra3f(pop,para);
    pop(:,4)=z';
    [cs,inds]=max(fpop);bchrom=pop(inds,:);
    % tournament selection
    toursize=5;
    players=ceil(pops*rand(pops,toursize));
    scores=fpop(players);
    [a,m]=max(scores');
    pind=zeros(1,pops);
    for ii=1:pops
        pind(ii)=players(ii,m(ii));
        parent(ii,:)=pop(pind(ii),:);
    end
    %arithmatic crossover
    offs=cross_singlepoint(parent,crossprop);
%     offs=cross1(parent,crossprop);
    %mutate1=uniform mutation.
    moffs=mutate1(offs,mutprop);
    pop=moffs;
    [mm,z]=fitnesstra3f(pop,para);
    pop(:,4)=z';
    maxf(it)=max(mm);
    [bfit,bind]=max(mm);
    bsol=pop(bind,:);    % best solution.
    rec=recor(bsol,para);
    trec(it)=rec(1);
    qrec(it)=rec(2);
    drec(it)=rec(3);
    torrec(it)=rec(4);
    if mm(inds) < cs
    pop(inds,:)=bchrom;
    end
    %*************************plot results

    qs1=para(1);qs2=para(2);qs3=para(3);
    qm1=bsol(1);qm2=bsol(2);qm3=bsol(3);
    [w,conff]=invkin3(para(4),para(5),bsol(4));
    qg1=conff(1);qg2=conff(2);qg3=conff(3);

    [xs1,ys1]=pol2cart(qs1,l1);
    [xs2,ys2]=pol2cart(qs2+qs1,l2);
    [xs3,ys3]=pol2cart(qs3+qs2+qs1,l3);

    xxs1=linspace(0,xs1);
    yys1=linspace(0,ys1);
    xxs2=linspace(0,xs2);xxs2=xxs2+xs1;
    yys2=linspace(0,ys2);yys2=yys2+ys1;
    xxs3=linspace(0,xs3);xxs3=xxs3+xs2+xs1;
    yys3=linspace(0,ys3);yys3=yys3+ys2+ys1;

    [xm1,ym1]=pol2cart(qm1,l1);
    [xm2,ym2]=pol2cart(qm2+qm1,l2);
    [xm3,ym3]=pol2cart(qm3+qm2+qm1,l3);

    xxm1=linspace(0,xm1);
    yym1=linspace(0,ym1);
    xxm2=linspace(0,xm2);xxm2=xxm2+xm1;
    yym2=linspace(0,ym2);yym2=yym2+ym1;
    xxm3=linspace(0,xm3);xxm3=xxm3+xm2+xm1;
    yym3=linspace(0,ym3);yym3=yym3+ym2+ym1;

    [xg1,yg1]=pol2cart(qg1,l1);
    [xg2,yg2]=pol2cart(qg2+qg1,l2);
    [xg3,yg3]=pol2cart(qg3+qg2+qg1,l3);

    xxg1=linspace(0,xg1);
    yyg1=linspace(0,yg1);
    xxg2=linspace(0,xg2);xxg2=xxg2+xg1;
    yyg2=linspace(0,yg2);yyg2=yyg2+yg1;
    xxg3=linspace(0,xg3);xxg3=xxg3+xg2+xg1;
    yyg3=linspace(0,yg3);yyg3=yyg3+yg2+yg1;
    xt=[xxs1;xxs2;xxs3;xxm1;xxm2;xxm3;xxg1;xxg2;xxg3];
    yt=[yys1;yys2;yys3;yym1;yym2;yym3;yyg1;yyg2;yyg3];
    cond=[confi,conff];
    chrom=[bsol(1:3),bsol(5:end)];
    kk=trajt3(cond,chrom);
    pq=kk(1:3,[5,7,10,13,15,17,20,23,25,27,30,33,35]);
    pcart=forkin3(kk(1,:),kk(2,:),kk(3,:));
    px=pcart(1,:);py=pcart(2,:);
    if it==maxgen
      [xxt,yyt]=angls2links(pq);
      figure,plot(xxt',yyt')
      xlabel('x(m)')
      ylabel('y(m)')
      hold on
    end
    plot(px,py)
    hold on
    axis([-2.7 2.7 -2.7 2.7])
    text(0,2.6,['gen. no.',num2str(it)])
    plot(xt',yt')
    hold off
    pause(0)
end
toc
e=[1:maxgen];
figure,plot(e,1./maxf)
xlabel('generation')
ylabel('min. fitness')

tt=torque3(kk);

t1=bsol(8);ti1=linspace(0,t1,20);
ti2=bsol(9);ti2=linspace(t1,ti2+t1,20);
time=[ti1,ti2];

figure,plot(time,kk(1,:),'r--',time,kk(2,:),'g--+',time,kk(3,:),'b-*',time(20),kk(1,20),'ko',time(20),kk(2,20),'ko',time(20),kk(3,20),'ko')
h = legend('joint 1','joint 2','joint 3',2);
xlabel('Time(s)')
ylabel('joint angle(rad)')
figure,plot(time,kk(4,:),'r--',time,kk(5,:),'g--+',time,kk(6,:),'b-*',time(20),kk(4,20),'ko',time(20),kk(5,20),'ko',time(20),kk(6,20),'ko')
h = legend('joint 1','joint 2','joint 3',2);
xlabel('Time(s)')
ylabel('joint vilocity(rad/s)')
figure,plot(time,kk(7,:),'r--',time,kk(8,:),'g--+',time,kk(9,:),'b-*',time(20),kk(7,20),'ko',time(20),kk(8,20),'ko',time(20),kk(9,20),'ko')
h = legend('joint 1','joint 2','joint 3',2);
xlabel('Time(s)')
ylabel('joint acceleration(rad/s^2)')

figure,plot(time,tt(1,:),'r--',time,tt(2,:),'g--+',time,tt(3,:),'b-*',time(20),tt(1,20),'ko',time(20),tt(2,20),'ko',time(20),tt(3,20),'ko')
h = legend('joint 1','joint 2','joint 3',2);
xlabel('Time(s)')
ylabel('joint tourque(N.m)')

figure,plot(e,trec)
xlabel('generation')
ylabel('consumed time for point to point motion(s)')
figure,plot(e,qrec)
xlabel('generation')
ylabel('total joint distance(rad)')
figure,plot(e,drec)
xlabel('generation')
ylabel('total cartesian trajectory length(m)')
figure,plot(e,torrec)
xlabel('generation')
ylabel('total excessive torque(N.m)')
bsol'


function u = minL2Potts2DADMM(f, gamma, varargin)
%minL2Potts2DADMM A ADMM splitting strategy for the two-dimensional vector valued L^2-Potts
%problem
%
%Description:
% Minimizes the (2D) Potts problem
%
%  \gamma \| u \|_0 + \| A u - f \|_p^p -> min
%
% using ADMM splitting an dynamic programming
%
%Reference:
% M. Storath, A. Weinmann
% Fast partitioning of vector-valued images",
% SIAM Journal on Imaging Sciences, 2014

% written by M. Storath
% $Date: 2014-06-30 11:26:34 +0200 (Mo, 30. Jun 2014) $	$Revision: 99 $

[m,n,~] = size(f);

% parse options
ip = inputParser;
addParamValue(ip, 'muInit', gamma*1e-2);
addParamValue(ip, 'muStep', 2);
addParamValue(ip, 'tol', 1e-10);
addParamValue(ip, 'isotropic', true);
addParamValue(ip, 'verbose', false);
addParamValue(ip, 'weights', ones(m,n));
addParamValue(ip, 'multiThreading', true);
addParamValue(ip, 'quantization', true);
addParamValue(ip, 'useADMM', true);
parse(ip, varargin{:});
par = ip.Results;

% check args
assert(par.muStep > 1, 'Variable muStep must be > 1.');
assert(all(par.weights(:) >= 0), 'Weights must be >= 0.');
assert(par.tol > 0, 'Stopping tolerance must be > 0.');
assert(par.muInit > 0, 'muInit must be > 0.');

% cast data to PLImage
plf = pottslab.PLImage(f);

% main program (calls Java routines)
if par.isotropic
    % near-isotropic discretization
    omega(1) = sqrt(2.0) - 1.0;
    omega(2) = 1.0 - sqrt(2.0)/2.0;
    % alternative neighborhood weights
    %omega(1) = (2 * sqrt(2.0) - 1.0)/3;
    %omega(2) = (2 - sqrt(2.0))/3;
    plu = pottslab.JavaTools.minL2PottsADMM8(plf, gamma, par.weights, par.muInit, par.muStep, par.tol, par.verbose, par.multiThreading, par.useADMM, omega);

else
    % anisotropic discretization
    plu = pottslab.JavaTools.minL2PottsADMM4(plf, gamma, par.weights, par.muInit, par.muStep, par.tol, par.verbose, par.multiThreading, par.useADMM);
end

% reshape the 1D array given by .toDouble()
u = reshape( plu.toDouble(), size(f) );

% to remove small remaining variations in result (algorithm works with floats)
if par.quantization
    u = round(u * 255)/255;
end

end


% Script demonstrating usage of the olcdl_sgd_msk function.
%
% Author: Brendt Wohlberg <brendt@lanl.gov>
%         Jialin Liu <danny19921123@gmail.com>
%
% This file is part of the SPORCO library. Details of the copyright
% and user license can be found in the 'Copyright' and 'License' files
% distributed with the library.

% NB: This script uses only 5 training images to avoid running for
% longer than is appropriate for a demo script. Note, however, that
% the advantages of the online learning algorithms relative to the
% batch algorithms are not apparent for such a small training set.

% Training images
S0 = zeros(512, 512, 5, 'single');
S0(:,:,1) = single(stdimage('lena.grey')) / 255;
S0(:,:,2) = single(stdimage('barbara.grey')) / 255;
S0(:,:,3) = single(stdimage('kiel.grey')) / 255;
S0(:,:,4) = single(rgb2gray(stdimage('mandrill'))) / 255;
tmp = single(stdimage('man.grey')) / 255;
S0(:,:,5) = tmp(101:612, 101:612);

%Reduce images size to speed up demo script
tmp = zeros(256, 256, 5, 'single');
for k = 1:size(S0,3),
  tmp(:,:,k) = imresize(S0(:,:,k), 0.5);
end
S0 = tmp;

% Apply 10% salt & pepper noise to images
[Sn, wn] = spnoise(S0, 0.1);

% Filter input images and compute highpass images
Sl = zeros(size(Sn), 'single');
Sh = zeros(size(Sn), 'single');
opt_filt = [];
opt_filt.Verbose = 0;
opt_filt.MaxMainIter = 500;
opt_filt.RelStopTol = 1e-3;
for index = 1:size(Sn,3),
    sn = Sn(:,:,index);
    opt_filt.DatFidWeight = ~wn(:,:,index);
    sl = l2tvdenoise(sn, 1, opt_filt);
    sh = sn - sl;
    Sl(:,:,index) = sl;
    Sh(:,:,index) = sh;
end

% Construct initial dictionary
D0 = zeros(8,8,32, 'single');
D0(3:6,3:6,:) = single(randn(4,4,32));

% Set up olcdl parameters
lambda = 0.2;
opt = [];
opt.MaxMainIter = 5*size(S0,3);

% Do dictionary learning
W = ~wn;
[D, optinf] = olcdl_sgd_msk(D0, Sh, lambda, W, opt);

% Display learned dictionary
figure;
imdisp(tiledict(D));

% Plot functional value evolution
figure;
plot(optinf.itstat(:,2));
xlabel('Iterations');
ylabel('Functional value');


function traj = dirTrans_Euler(config)

n = config.grid.nTrajPts;

% Create the initial guess:
guess.time = linspace(config.guess.time(1),config.guess.time(end),n);
guess.state = interp1(...
    config.guess.time', config.guess.state', guess.time')';
guess.control = interp1(...
    config.guess.time', config.guess.control', guess.time')';

% Create a list of all linear constraints, then add function handles:
[problem, pack] = buildConstraints(guess,config);
problem.objective = @(z)( costFunctionWrapper(z,pack) );
problem.nonlcon = @(z)( nonLinCon(z,pack, config) );

% Solve using fmincon:
[zSoln,fSoln,exitFlag] = fmincon(problem);

% Post-processing:
[t,x,u] = unPackDecVar(zSoln,pack);

traj.time = linspace(t(1),t(2),n);
traj.state = x;
traj.control = u;
traj.objVal = fSoln;
traj.exitFlag = exitFlag;
traj.interp.state = @(tt)( interp1(traj.time',traj.state',tt')' );
traj.interp.control = @(tt)( interp1(traj.time',traj.control',tt')' );

end

function [C, Ceq] = nonLinCon(z,pack,config)
%
% This function enforces the dynamics of the cart-pole system
%

% Based on the Trapazoid Method for discretization, as defined in Bett's book,
% chapter 4.

n = pack.nState(2);
[t,x,u] = unPackDecVar(z,pack);
dt = (t(2)-t(1))/(n-1);

% Evaluate the dynamics at each collocation point
dx = cartPoleDynamics(x,[u; zeros(size(u))],config.dyn);

% Trapazoid rule:
idxLow = 1:(n-1);
idxUpp = 2:n;
intStateTrap = 0.5*dt*(dx(:,idxLow) + dx(:,idxUpp));
intStateCol = x(:,idxUpp)-x(:,idxLow);

% Defect constraint:
defect = intStateTrap - intStateCol;
%
% % user-defined boundary constraints:
% [bndIneq, bndEq] = boundaryConstraint(t,x(:,1),x(:,end),config.userData);

C = [];%bndIneq;
Ceq = [reshape(defect,numel(defect),1)];% bndEq];

end

function cost = costFunctionWrapper(z,pack)

[t,x,u] = unPackDecVar(z,pack);

% Trapazoid rule to integrate cost function:
tt = linspace(t(1),t(end),pack.nState(2));  %Time vector
dc = costFunction(tt,x,u);
nTime = size(x,2);
dt = (t(2)-t(1))/(nTime-1);
w = ones(nTime,1); w([1,end]) = 0.5;
cost = dt*dc*w;

end


function [dynpcm2] = bar2dynpcm2(bar)
% Convert pressure from bar to dynes/cm^2.
% Chad Greene 2012
dynpcm2 = bar*1.00000e+6;


function [min_v,min_i,min_diff] = local_min(F,S,ep)
  % LOCAL_MIN  find values and indices of local minima of a scalar field
  % defined on a mesh
  %
  % [min_v,min_i,min_diff] = local_min(F,S,ep)
  %
  % Inputs:
  %   F #faces by 3 list of triangle indices
  %   S #vertices by 1 list of scalar values
  % Outputs:
  %   min_v  #minima list of values at local minima
  %   min_i  #minima list of indices into S of local minima
  %   min_diff #minima list of min difference between each minima and its
  %     neighbors
  %

  if ~exist('ep','var')
    ep = 0;
  end

  % number of vertices
  n = numel(S);
  if size(F,2) == 2
    E = F;
  else
    E = edges(F);
  end
  E = [E;fliplr(E)];
  A = sparse( ...
    E(:,1), ...
    E(:,2), ...
    S(E(:,1))-S(E(:,2)), ...
    n,n);
  minA = minnz(A)';
  [min_i,~] = find(minA>-ep);
  min_diff = minA(min_i);
  min_v = S(min_i);

end


function jed = yjf_to_jed_common ( y, j, f )

%*****************************************************************************80
%
%% YJF_TO_JED_COMMON converts a Common YJF date to a JED.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, J, real F, the YJF date.
%
%    Output, real JED, the Julian Ephemeris Date.
%

%
%  Copy the input.
%
  y1 = y;
  j1 = j;
  f1 = f;
%
%  Check the input.
%
  [ y1, j1, f1, ierror ] = yjf_check_common ( y1, j1, f1 );

  if ( ierror ~= 0 )
    jed = -1.0;
    return
  end
%
%  Convert the input.
%
  [ y2, m2, d2, f2 ] = yjf_to_ymdf_common ( y1, j1, f1 );

  jed = ymdf_to_jed_common ( y2, m2, d2, f2 );

  return
end


% TEST_WAVELET_2D Test case for WAVELET_2D
%
% See also
%   WAVELET_2D
classdef test_wavelet_2d < matlab.unittest.TestCase
    methods(Test)

        function testWithNoOptions(testcase)
            %% define
            x = rand(64, 64);
            filters = morlet_filter_bank_2d(size(x));
            %% with no options
            [x_phi, x_psi] = ...
                wavelet_2d(x, filters);
            %% check number of high pass filter
            J = filters.meta.J;
            L = filters.meta.L;
            Q = filters.meta.Q;
            expected = J*L*Q;
            actual = numel(x_psi.signal);
            %% assert
            testcase.assertEqual(expected, actual);

        end

         function testWithRandomOptions(testcase)
            for i = 1:32
                %%
                sz = 1 + floor(128*rand(1,2));
                %% define
                x = rand(sz);

                white_list={'x_resolution','oversampling'};
                type={'d','d'};
                values={{1,2,4,8,16},{0,1}};

                options_W = generate_random_options(white_list,type,values);

                filters = morlet_filter_bank_2d(sz);
                %% with no options
                [x_phi, x_psi] = ...
                    wavelet_2d(x, filters,options_W);
                %% check number of high pass filter
                J = filters.meta.J;
                L = filters.meta.L;
                Q = filters.meta.Q;
                expected = J*L*Q;
                actual = numel(x_psi.signal);
                %% assert
                testcase.assertEqual(expected, actual);

            end
         end

        function testWithRandomSize(testcase)
            for i = 1:32
                %%
                sz = 1 + floor(128*rand(1,2));
                %% define
                x = rand(sz);
                filters = morlet_filter_bank_2d(sz);
                %% with no options
                [x_phi, x_psi] = ...
                    wavelet_2d(x, filters);
                %% check number of high pass filter
                J = filters.meta.J;
                L = filters.meta.L;
                Q = filters.meta.Q;
                expected = J*L*Q;
                actual = numel(x_psi.signal);
                %% assert
                testcase.assertEqual(expected, actual);

            end

        end
    end
end