function test_bug2040

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY nansum

[ftver, ftpath] = ft_version;

p  = tempname;
p1 = fullfile(p, 'mexfile');
p2 = fullfile(p, 'mfile');
p3 = fullfile(p, 'stats');

mkdir(p);
mkdir(fullfile(p1));
mkdir(fullfile(p2));
mkdir(fullfile(p3));

% separate the three implementations
cd(p1);
copyfile(fullfile(ftpath, 'src', ['nansum.' mexext]), '.');
cd(p2);
copyfile(fullfile(ftpath, 'src', 'nansum.m'), '.');
cd(p3);
% don't copy anything here, assume that the Mathworks signal processing toolbox is on the path

% make some data
n = 10;
x{1} = randn(n,1);
x{2} = x{1}';
x{3} = randn(n,n);
x{4} = randn(n,n,n);
x{5} = randn(n,n,n,n);

originalpath = path;
restoredefaultpath

% compute the solution for the three implementations
cd(p1);
s1 = cellfun(@nansum, x, 'UniformOutput', false);
cd(p2);
s2 = cellfun(@nansum, x, 'UniformOutput', false);
cd(p3);
s3 = cellfun(@nansum, x, 'UniformOutput', false);

% the solutions should all be equal
assert(isequal(s1, s2));
assert(isequal(s2, s3));

path(originalpath);


function Volume=polygon2voxel_double(FacesA,FacesB,FacesC,VerticesX,VerticesY,VerticesZ,VolumeSize,Wrap)
Vertices=[VerticesX(:) VerticesY(:) VerticesZ(:)]-1;

% List with all vertices coordinates of a face
FaceVertices=[Vertices(FacesA,:) Vertices(FacesB,:) Vertices(FacesC,:)];
Volume=false(VolumeSize);
Volume=DrawSplitFaces(FaceVertices,Volume,Wrap);

function Volume=DrawSplitFaces(FaceVertices,Volume,Wrap)
VolumeSize=size(Volume);
% Calculate squared edge distances
dist1=(FaceVertices(:,1)-FaceVertices(:,4)).*(FaceVertices(:,1)-FaceVertices(:,4))+(FaceVertices(:,2)-FaceVertices(:,5)).*(FaceVertices(:,2)-FaceVertices(:,5))+(FaceVertices(:,3)-FaceVertices(:,6)).*(FaceVertices(:,3)-FaceVertices(:,6));
dist2=(FaceVertices(:,7)-FaceVertices(:,4)).*(FaceVertices(:,7)-FaceVertices(:,4))+(FaceVertices(:,8)-FaceVertices(:,5)).*(FaceVertices(:,8)-FaceVertices(:,5))+(FaceVertices(:,9)-FaceVertices(:,6)).*(FaceVertices(:,9)-FaceVertices(:,6));
dist3=(FaceVertices(:,1)-FaceVertices(:,7)).*(FaceVertices(:,1)-FaceVertices(:,7))+(FaceVertices(:,2)-FaceVertices(:,8)).*(FaceVertices(:,2)-FaceVertices(:,8))+(FaceVertices(:,3)-FaceVertices(:,9)).*(FaceVertices(:,3)-FaceVertices(:,9));

% Calculate mFaceVertices(:,1) distance
maxdist=max([dist1(:) dist2(:),dist3(:)],[],2);

% Draw triangle if distance <=0.5 pixel
check=maxdist>0.5;

FVR=FaceVertices(~check,:);
% Select Vertices which must be split
FaceVertices=FaceVertices(check,:);
if(~isempty(FaceVertices))
    dist1=dist1(check);
    dist2=dist2(check);
    dist3=dist3(check);

    DX=(FaceVertices(:,1)+FaceVertices(:,4))/2; DY=(FaceVertices(:,2)+FaceVertices(:,5))/2; DZ=(FaceVertices(:,3)+FaceVertices(:,6))/2;
    FA1=[DX,DY,DZ,FaceVertices(:,4),FaceVertices(:,5),FaceVertices(:,6),FaceVertices(:,7),FaceVertices(:,8),FaceVertices(:,9)];
    FB1=[FaceVertices(:,1),FaceVertices(:,2),FaceVertices(:,3),DX,DY,DZ,FaceVertices(:,7),FaceVertices(:,8),FaceVertices(:,9)];

    DX=(FaceVertices(:,1)+FaceVertices(:,7))/2; DY=(FaceVertices(:,2)+FaceVertices(:,8))/2; DZ=(FaceVertices(:,3)+FaceVertices(:,9))/2;
    FA2=[DX,DY,DZ,FaceVertices(:,4),FaceVertices(:,5),FaceVertices(:,6),FaceVertices(:,7),FaceVertices(:,8),FaceVertices(:,9)];
    FB2=[FaceVertices(:,1),FaceVertices(:,2),FaceVertices(:,3),FaceVertices(:,4),FaceVertices(:,5),FaceVertices(:,6),DX,DY,DZ];

    DX=(FaceVertices(:,7)+FaceVertices(:,4))/2; DY=(FaceVertices(:,8)+FaceVertices(:,5))/2; DZ=(FaceVertices(:,9)+FaceVertices(:,6))/2;
    FA3=[FaceVertices(:,1),FaceVertices(:,2),FaceVertices(:,3),DX,DY,DZ,FaceVertices(:,7),FaceVertices(:,8),FaceVertices(:,9)];
    FB3=[FaceVertices(:,1),FaceVertices(:,2),FaceVertices(:,3),FaceVertices(:,4),FaceVertices(:,5),FaceVertices(:,6),DX,DY,DZ];

    DX=(FaceVertices(:,1)+FaceVertices(:,7))/2; DY=(FaceVertices(:,2)+FaceVertices(:,8))/2; DZ=(FaceVertices(:,3)+FaceVertices(:,9))/2;
    FA4=[DX,DY,DZ,FaceVertices(:,4),FaceVertices(:,5),FaceVertices(:,6),FaceVertices(:,7),FaceVertices(:,8),FaceVertices(:,9)];
    FB4=[FaceVertices(:,1),FaceVertices(:,2),FaceVertices(:,3),FaceVertices(:,4),FaceVertices(:,5),FaceVertices(:,6),DX,DY,DZ];

    dist12=dist1>dist2;
    dist12n=~dist12;
    FA1(dist12n,:)=FA3(dist12n,:);
    FB1(dist12n,:)=FB3(dist12n,:);
    FA2(dist12n,:)=FA4(dist12n,:);
    FB2(dist12n,:)=FB4(dist12n,:);
    dist1(dist12n,:)=dist2(dist12n,:);

    dist13=dist1>dist3;
    dist13n=~dist13;
    FA1(dist13n,:)=FA2(dist13n,:);
    FB1(dist13n,:)=FB2(dist13n,:);

    FaceVertices=[FA1;FB1];

    % Split / Draw Vertices
    Volume=DrawSplitFaces(FaceVertices,Volume,Wrap);
end

% Draw remaining faces
FaceVertices=FVR;

if(Wrap==0)
    % Calculate 1D volume indices
    indexA=mindex3(floor(FaceVertices(:,1)+0.5),floor(FaceVertices(:,2)+0.5), floor(FaceVertices(:,3)+0.5), VolumeSize(1), VolumeSize(2), VolumeSize(3), Wrap);
    indexB=mindex3(floor(FaceVertices(:,4)+0.5),floor(FaceVertices(:,5)+0.5), floor(FaceVertices(:,6)+0.5), VolumeSize(1), VolumeSize(2), VolumeSize(3), Wrap);
    indexC=mindex3(floor(FaceVertices(:,7)+0.5),floor(FaceVertices(:,8)+0.5), floor(FaceVertices(:,9)+0.5), VolumeSize(1), VolumeSize(2), VolumeSize(3), Wrap);

    % Remove outside vertices
    checkA=(FaceVertices(:,1)<0)|(FaceVertices(:,2)<0)|(FaceVertices(:,3)<0)|(FaceVertices(:,1)>(VolumeSize(1)-1))|(FaceVertices(:,2)>(VolumeSize(2)-1))|(FaceVertices(:,3)>(VolumeSize(3)-1));
    checkB=(FaceVertices(:,4)<0)|(FaceVertices(:,5)<0)|(FaceVertices(:,6)<0)|(FaceVertices(:,4)>(VolumeSize(1)-1))|(FaceVertices(:,5)>(VolumeSize(2)-1))|(FaceVertices(:,6)>(VolumeSize(3)-1));
    checkC=(FaceVertices(:,7)<0)|(FaceVertices(:,8)<0)|(FaceVertices(:,9)<0)|(FaceVertices(:,7)>(VolumeSize(1)-1))|(FaceVertices(:,8)>(VolumeSize(2)-1))|(FaceVertices(:,9)>(VolumeSize(3)-1));
    indexA(checkA)=[];
    indexB(checkB)=[];
    indexC(checkC)=[];

    % Draw the vertices
    Volume(indexA)=true;
    Volume(indexB)=true;
    Volume(indexC)=true;
else
     Volume(mindex3(floor(FaceVertices(:,1)+0.5),floor(FaceVertices(:,2)+0.5), floor(FaceVertices(:,3)+0.5), VolumeSize(1), VolumeSize(2), VolumeSize(3), Wrap))=1;
     Volume(mindex3(floor(FaceVertices(:,4)+0.5),floor(FaceVertices(:,5)+0.5), floor(FaceVertices(:,6)+0.5), VolumeSize(1), VolumeSize(2), VolumeSize(3), Wrap))=1;
     Volume(mindex3(floor(FaceVertices(:,7)+0.5),floor(FaceVertices(:,8)+0.5), floor(FaceVertices(:,9)+0.5), VolumeSize(1), VolumeSize(2), VolumeSize(3), Wrap))=1;
end

function index=mindex3(x, y, z, sizx, sizy, sizz, Wrap)
if(Wrap==1)
    % Positive modules
    x=mod(x,sizx);
    y=mod(y,sizy);
    z=mod(z,sizz);
elseif(Wrap>1)
    % Clamp
    x=max(x,0); x=min(x,sizx-1);
    y=max(y,0); y=min(y,sizy-1);
    z=max(z,0); z=min(z,sizz-1);
end
index=z*sizx*sizy+y*sizx+x;
% matlab
index=index+1;


% =========================================================================
% An example code for the algorithm proposed in
%
%   Zhuolin Jiang, Zhe Lin, Larry S. Davis.
%   "Learning A Discriminative Dictionary for Sparse Coding via Label
%    Consistent K-SVD", CVPR 2011.
%
% Author: Zhuolin Jiang (zhuolin@umiacs.umd.edu)
% Date: 10-16-2011
% =========================================================================

clear all;
clc;
addpath(genpath('.\ksvdbox'));  % add K-SVD box
addpath(genpath('.\OMPbox')); % add sparse coding algorithem OMP
load('.\trainingdata\featurevectors.mat','training_feats', 'testing_feats', 'H_train', 'H_test');

%% constant
sparsitythres = 30; % sparsity prior
sqrt_alpha = 4; % weights for label constraint term
sqrt_beta = 2; % weights for classification err term
dictsize = 570; % dictionary size
iterations = 50; % iteration number
iterations4ini = 20; % iteration number for initialization

%% dictionary learning process
% get initial dictionary Dinit and Winit
fprintf('\nLC-KSVD initialization... ');
[Dinit,Tinit,Winit,Q_train] = initialization4LCKSVD(training_feats,H_train,dictsize,iterations4ini,sparsitythres);
fprintf('done!');

% run LC K-SVD Training (reconstruction err + class penalty)
fprintf('\nDictionary learning by LC-KSVD1...');
[D1,X1,T1,W1] = labelconsistentksvd1(training_feats,Dinit,Q_train,Tinit,H_train,iterations,sparsitythres,sqrt_alpha);
save('.\trainingdata\dictionarydata1.mat','D1','X1','W1','T1');
fprintf('done!');

% run LC k-svd training (reconstruction err + class penalty + classifier err)
fprintf('\nDictionary and classifier learning by LC-KSVD2...')
[D2,X2,T2,W2] = labelconsistentksvd2(training_feats,Dinit,Q_train,Tinit,H_train,Winit,iterations,sparsitythres,sqrt_alpha,sqrt_beta);
save('.\trainingdata\dictionarydata2.mat','D2','X2','W2','T2');
fprintf('done!');

%% classification process
[prediction1,accuracy1] = classification(D1, W1, testing_feats, H_test, sparsitythres);
fprintf('\nFinal recognition rate for LC-KSVD1 is : %.03f ', accuracy1);

[prediction2,accuracy2] = classification(D2, W2, testing_feats, H_test, sparsitythres);
fprintf('\nFinal recognition rate for LC-KSVD2 is : %.03f ', accuracy2);


filename='Cantilever_tetrahedra_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'SLERP';
incrementFactor = 1;
designVariable = 'LevelSet';
filterType = 'P1';

nsteps = 5;
Vfrac_final = 0.05;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-2;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function [mclab, mcprc, allprc, trainw] = segmentation2labels(imsegs, smap)

nseg = max(smap);

mclab = zeros(nseg, 1);
mcprc = zeros(nseg, 1);
allprc = zeros(nseg, 7);
trainw = zeros(nseg, 1);

imsegs.npixels = imsegs.npixels(:);
imsegs.labels = imsegs.labels(:);

for s = 1:nseg
    ind = find(smap==s);
    for k = 1:7
        allprc(s, k) = allprc(s, k) + sum((imsegs.labels(ind)==k).*imsegs.npixels(ind));
    end
    allprc(s, :) = allprc(s, :) / max(sum(allprc(s, :)),1E-6);
    [mcprc(s), mclab(s)] = max(allprc(s, :));
    trainw(s) = sum(imsegs.npixels(ind))/sum(imsegs.npixels);
end


%% notes

% How lookback/horizon changes accuracy
% exploratory data analysis
% How does sector affect price movements
% Try a mixture of SVM models?

%%

SNPIndices = zeros(6356, 1);
otherIndices = [];

for i=1:505
    boolIdx = strcmp(Symbol(i), names);
    if (any(boolIdx))
        SNPIndices = (SNPIndices | boolIdx);
        otherIndices = [otherIndices i];
    end
end

%%
% have names and Sector, Names

newSectors = cell(494, 1);

for i=1:494

    linInd = find(strcmp(Symbol(i), NAMES));
    newSectors{linInd} = Sector{i};

end

%% data loading and plotting tests

today = 30;
lookback = 10;
horizon = 2;
[X, y, ~, ~] = loadData(today, lookback, horizon);

plot(today-lookback+1:today, X(1,:));
hold on;
plot(today+horizon, y(1), 'r*', 'MarkerSize', 12);

%%
%% try some clustering
k = 4;
[idx, means] = kmeans(PRICE_HISTORY, k);

%%
figure();
for i=1:k
    subplot(2, 3, i);
    hold on;
    plot(PRICE_HISTORY(idx == i, :)');
end

%%
figure();
for i=1:k
    subplot(2, 3, i);
    hold on;
    plot(means(i,:));
end

%%

[X, y] = loadData2(30, 10);


function movePTPEllipse_YZ_1(iiwa,c,ratio,theta,velocity,accel,TefTool)
%% This funciton is used to
% Move end-effector of the robot on an ellipse the plane of the ellipse is
% parallel to the XY plane of the robot base.

%% Syntax:
% movePTPEllipse_YZ(iiwa,c,ratio,theta,velocity,accel,TefTool)

%% Arreguments:
% iiwa: KST obejct.
% c: 2x1 vector, is a vector defining the displacment of the the center
% point of the ellipse in the YZ plane in relation to the starting point of the ellipse,
% position units in (mm).
% ratio: the radious ratio (a/b) of the ellipse.
% theta: is the angle of the ellipe part , for drawing a complete
% ellipse this angle is equal to 2*pi.
% TefTool: 4X4 Transform matrix of the end-efector in the reference frame
% of the flange of the KUKA iiwa, position units in (mm).
% accel: is the acceleration (mm/sec2).
% velocity: is the motion velocity (mm/sec).

% Copy right: Mohammad SAFEEA
% 30-April-2017

%% Convert inputs to a column vectors
c=colVec(c);
ratio=colVec(ratio);
theta=colVec(theta);
velocity=colVec(velocity);
accel=colVec(accel);

%% Check input variables
if(size(c,1)~=2)
fprintf('Error: the vector (c) shall be a 2x1 vector \n');
return;
end

if(size(ratio,1)~=1)
fprintf('Error: the ratio shall be a scalar  \n');
return;
end

if(size(theta,1)~=1)
fprintf('Error: the variable (theta) shall be a scalar  \n');
return;
end

if(size(TefTool,1)~=4)
fprintf('Error: the transform matrix (TefTool) shall be 4x4  \n');
return;
end

if(size(TefTool,2)~=4)
fprintf('Error: the transform matrix (TefTool) shall be 4x4  \n');
return;
end

if(size(velocity,1)~=1)
fprintf('Error: the velocity on the path shall be a scalar  \n');
return;
end

if(size(accel,1)~=1)
fprintf('Error: the acceleration on the path shall be a scalar  \n');
return;
end

dir=[0;1;0];
c=[0;
    c(1);
    c(2)];

% perform the elliptical motion
movePTPEllipse(iiwa,c,dir,ratio,theta,velocity,accel,TefTool);
end

function y=colVec(x)
if size(x,2)==1
    y=x;
else
    y=x';
end
end


%LandmarkMap Map of planar point landmarks
%
% A LandmarkMap object represents a square 2D environment with a number of landmark
% landmark points.
%
% Methods::
%   plot      Plot the landmark map
%   landmark   Return a specified map landmark
%   display   Display map parameters in human readable form
%   char      Convert map parameters to human readable string
%
% Properties::
%   map         Matrix of map landmark coordinates 2xN
%   dim         The dimensions of the map region x,y in [-dim,dim]
%   nlandmarks   The number of map landmarks N
%
% Examples::
%
% To create a map for an area where X and Y are in the range -10 to +10 metres
% and with 50 random landmark points
%        map = LandmarkMap(50, 10);
% which can be displayed by
%        map.plot();
%
% Reference::
%
%   Robotics, Vision & Control, Chap 6,
%   Peter Corke,
%   Springer 2011
%
% See also RangeBearingSensor, EKF.

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

classdef LandmarkMap < handle
% TODO:
% add a name property, show in char()

    properties
        map    % map landmarks
        dim         % map dimension
        nlandmarks   % number of landmarks in map

        verbose
    end

    methods

        % constructor
        function map = LandmarkMap(nlandmarks, varargin)
        %LandmarkMap.LandmarkMap Create a map of point landmark landmarks
        %
        % M = LandmarkMap(N, DIM, OPTIONS) is a LandmarkMap object that represents N random point landmarks
        % in a planar region bounded by +/-DIM in the x- and y-directions.
        %
        % Options::
        % 'verbose'    Be verbose

        %% TODO: dim can be a 4-vector

            opt = [];
            [opt,args] = tb_optparse(opt, varargin);
            map.verbose = opt.verbose;

            if ~isempty(args) && isnumeric(args{1})
                dim = args{1};
            else
                dim = 10;
            end
            map.dim = dim;
            map.nlandmarks = nlandmarks;
            map.map = dim * (2*rand(2, nlandmarks)-1);
            map.verbose = false;
        end

        function f = landmark(map, k)
            %LandmarkMap.landmark Get landmarks from map
            %
            % F = M.landmark(K) is the coordinate (2x1) of the K'th landmark (landmark).
            f = map.map(:,k);
        end

        function plot(map, varargin)
            %LandmarkMap.plot Plot the map
            %
            % M.plot() plots the landmark map in the current figure, as a square
            % region with dimensions given by the M.dim property.  Each landmark
            % is marked by a black diamond.
            %
            % M.plot(LS) as above, but the arguments LS
            % are passed to plot and override the default marker style.
            %
            % Notes::
            % - The plot is left with HOLD ON.
            clf
            d = map.dim;
            axis equal
            axis([-d d -d d]);
            xlabel('x');
            ylabel('y');

            if nargin == 1
                args = {'kh'};
            else
                args = varargin;
            end
            h = plot(map.map(1,:)', map.map(2,:)', args{:});
            set(h, 'Tag', 'map');
            grid on
            hold on
        end

        function show(map, varargin)
        %map.SHOW Show the landmark map
        %
        % Notes::
        % - Deprecated, use plot method.
            warning('show method is deprecated, use plot() instead');
            map.plot(varargin{:});
        end

        function verbosity(map, v)
            %map.verbosity Set verbosity
            %
            % M.verbosity(V) set verbosity to V, where 0 is silent and greater
            % values display more information.
            map.verbose = v;
        end

        function display(map)
            %map.display Display map parameters
            %
            % M.display() displays map parameters in a compact
            % human readable form.
            %
            % Notes::
            % - This method is invoked implicitly at the command line when the result
            %   of an expression is a LandmarkMap object and the command has no trailing
            %   semicolon.
            %
            % See also map.char.
            loose = strcmp( get(0, 'FormatSpacing'), 'loose');
            if loose
                disp(' ');
            end
            disp([inputname(1), ' = '])
            disp( char(map) );
        end % display()

        function s = char(map)
        %map.char Convert map parameters to a string
        %
        % s = M.char() is a string showing map parameters in
        % a compact human readable format.
            s = 'LandmarkMap object';
            s = char(s, sprintf('  %d landmarks', map.nlandmarks));
            s = char(s, sprintf('  dimension %.1f', map.dim));
        end

    end % method

end % classdef


function x = uncertain(x,varargin)
%UNCERTAIN Declares a variable as uncertain
%
%   F = UNCERTAIN(W) is used to describe the set of uncertain variables
%   in an uncertain program
%
%   INPUT
%    W : SDPVAR object or list of constraints
%    S : Optional distribution information for random uncertainty
%
%   OUTPUT
%    F : Constraint object
%
%   Uncertain is used to declare uncertain variables in robust
%   deterministic worst-case optimization. It can also be used to specify
%   uncertain random variables, and their associated distribution, to be
%   used in OPTIMIZER objects with the SAMPLE command.
%
%   EXAMPLE
%
%    Robust worst-case optimization
%
%    sdpvar x w
%    F = [x + w <= 1], W = [-0.5 <= w <= 0.5];
%    optimize([F,W,uncertain(w)],-x)
%
%    sdpvar x w
%    F = [x + w <= 1], W = [-0.5 <= w <= 0.5];
%    optimize([F,uncertain(W)],-x)
%
%   To specify random uncertainties, you specify the distribution, and all
%   distribution parameters following the syntax n the RANDOM command in
%   the Statistics Toolbox
%
%    sdpvar x w
%    F = [x + w <= 1, uncertain(w, 'uniform',0,1)];
%    P = optimizer([F,W,uncertain(w)],-x,[],w,x)
%    S = sample(P,10); % Sample ten instances and concatenate models
%    S([])             % Solve and return optimal x
%
%    Alternatively, you can specify a function handle which generates
%    samples. YALMIP will always send a trailing argument with dimensions
%
%    F = [x + w <= 1, uncertain(w,@mysampler,myarguments1,...)];
%
%    The standard random case above would thus be recovered with
%
%    F = [x + w <= 1, uncertain(w,@random,0,1)];
%
%
%   See also OPTIMIZE, ROBUSTMODEL, OPTIMIZER, SAMPLE

if nargin == 1 || ((nargin == 2) && strcmpi(varargin{1},'deterministic'))
    x.typeflag = 15;
    x.extra.distribution.name = 'deterministic';
    x = lmi(x);
else
    x.typeflag = 16;
    if isa(varargin{1},'function_handle')
         temp = {varargin{:},x.dim};
    else
        temp = {@random,varargin{:},x.dim};
    end
    x.extra.distribution.name = temp{1};
    x.extra.distribution.parameters = {temp{2:end-1}};
    try
        temp = feval(temp{:});
    catch
        disp(lasterr);
        error('Trial evaluation of attached sample generator failed.')
    end
    x = lmi(x);
end


% lssat;
% check saturation condition
%

cont=saturated;

if cont,
  % check boundary minimizer
  [fmin,i]=min(flist);
  if i==1 | i==s, cont=0; end;
end;

if cont,
  % select three points for parabolic interpolation
  aa=alist([i-1:i+1]);ff=flist([i-1:i+1]);

  % get divided differences
  f12=(ff(2)-ff(1))/(aa(2)-aa(1));
  f23=(ff(3)-ff(2))/(aa(3)-aa(2));
  f123=(f23-f12)/(aa(3)-aa(1));

  if f123>0,
    % parabolic minimizer
    alp=0.5*(aa(2)+aa(3)-f23/f123);
    alp=max(amin,min(alp,amax));
    alptol=small*(aa(3)-aa(1));
    saturated= ( abs(alist(i)-alp)<=alptol );
  else
    saturated=0;
  end;

  if prt>1 & ~saturated,
    disp('saturation check negative')
  end;
end;


function res = invert_nn_dw(net, ref, opts)

% setup param
[net,opts,x] = invert_nn_pre(net, ref, opts);
x0_size = cat(2,net.normalization.imageSize,opts.numRepeats);
x_momentum = zeros(x0_size, 'single') ;
load('x0_sigma.mat', 'x0_sigma');
y0 = repmat(ref, [1 1 1 opts.numRepeats]);
switch opts.task
    case {0,2}
        y0_sigma = norm(y0(:)) ;
    case {1}
        y0_sigma = 1;
end

%% --------------------------------------------------------------------
%%                                                 Perform optimisation
%% --------------------------------------------------------------------

% Run forward propogation once on the modified network before we
% begin backprop iterations - this is to exploit an optimization in
% vl_simplenn

% recored results
output = {} ;
prevlr = 0 ;

% Iterate until maxNumIterations to optimize the objective
% and generate the reconstuction
regu_out=[];
if ~isempty(opts.regu)
    regu_out=cell(1,numel(opts.regu));
end

res=[];
% visualize dropout mask
E=zeros(5,opts.maxNumIterations);
for t=1:opts.maxNumIterations

  % 1. Effectively does both forward and backward passes
  res = U_cnn_fb(net,res,x,opts.mask);

 % The current best feature we could generate
  switch net.cnn_mode
  case 'matconvnet'; y = res(end-1).x ;
  case 'caffe'; y = res.y ;
  end
  dr = zeros(size(x),'single'); % The derivative

  % 2. update the mask
  if ~isempty(opts.regu)
    for j=1:numel(opts.regu)
        regu_out{j} = res(opts.regu{j}{1}).x;
    end
  end

  % 3. calcuate gradient
  % 3.1 Data term
  E(1,t) = opts.lambdaD*res(end).x/(y0_sigma^2);

  % 3.2 Cost and derivative for TV\beta norm
  if opts.lambdaTV > 0
    tmp_x =x;
      if ~isempty(opts.im_w)
        tmp_x=reshape(reshape(x,[],3)*opts.im_w',size(x));
      end
    [r_,dr_] = tv(tmp_x,opts.TVbeta,opts.xmask) ;
    E(2,t) = opts.lambdaTV/2 * r_ ;
    dr = dr + opts.lambdaTV/2 * dr_ ;
  end
  % 3.2 Cost and derivative of L\beta norm
  if opts.lambdaL2 > 0
     tmp_x=x;
      if ~isempty(opts.im_w)
        tmp_x=reshape(reshape(x,[],3)*opts.im_w',size(x));
      end
    if ~isempty(opts.xmask)
        tmp_x = bsxfun(@times,x,opts.xmask);
    end
    if ~isempty(opts.regu_c)
      tmp_x = reshape(bsxfun(@minus,reshape(tmp_x,[],3),opts.regu_c),size(x));
    end
    r_ = sum(tmp_x(:).^opts.beta) ;
    dr_ = opts.beta * tmp_x.^(opts.beta-1) ;

    E(3,t) = opts.lambdaL2/2 * r_ ;
    dr = dr + opts.lambdaL2/2 * dr_ ;
  end

  E(2:3,t) = E(2:3,t) / (x0_sigma^2) ;
  E(4,t) = sum(E(1:3,t)) ;
  lr = opts.learningRate(min(t, numel(opts.learningRate))) ;

  % when the learning rate changes suddently, it is not
  % possible for the gradient to crrect the momentum properly
  % causing the algorithm to overshoot for several iterations
  if lr ~= prevlr
    fprintf('switching learning rate (%f to %f) and resetting momentum\n', ...
      prevlr, lr) ;
    x_momentum = 0 * x_momentum ;
    prevlr = lr ;
  end

  % x_momentum combines the current gradient and the previous gradients
  % with decay (opts.momentum)
  dr_d = opts.lambdaD*res(1).dzdx;
  %keyboard
  % [max(dr_d(:)) max(dr(:)/(x0_sigma^2)) max(opts.lambdaD*res(1).dzdx(:))]
  dsp_p=x0_sigma^2/y0_sigma^2;
  x_momentum = opts.momentum * x_momentum ...
      - lr * dr ...
      - (lr *  (x0_sigma^2/y0_sigma^2)*dr_d);

    % gradient clipping
    tmp_max =max(abs(x_momentum(:)));
    if opts.grad_ran(1)>=0
        if tmp_max>opts.grad_ran(2)
            x_momentum=x_momentum/tmp_max*opts.grad_ran(2);
        elseif tmp_max<opts.grad_ran(1)
            x_momentum=x_momentum/tmp_max*opts.grad_ran(1);
        end
    end
    tmp_max =max(abs(x_momentum(:)));

    if opts.dsp>=0
      fprintf('iter:%05d: max_grad=%3.4g, err_data=%5.4g, err_TV=%5.4g, err_L2=%5.4g, err_all=%5.4g;\n', t, tmp_max,E(1,t),E(2,t),E(3,t), E(4,t)) ;
  end

  % This is the main update step (we are updating the the variable
  % along the gradient
    if ~isempty(opts.xmask)
        x_momentum = bsxfun(@times,x_momentum,opts.xmask);
    end
    x = x + x_momentum ;

  % truncate output
  if opts.do_thres
      tmp_m = opts.denormalize(x);
      tmp_m(tmp_m<0)=0;
      tmp_m(tmp_m>255)=255;
      x = opts.normalize(tmp_m);
  end

  %% -----------------------------------------------------------------------
  %% Plots - Generate several plots to keep track of our progress
  %% -----------------------------------------------------------------------

  if opts.dsp>0 && mod(t-1,opts.dsp)==0
    figure(1) ; clf ;
    output{end+1} = opts.denormalize(x) ;
    subplot(3,2,[1 3]) ;
    if opts.numRepeats > 1
      vl_imarraysc(output{end}) ;
    else
      imagesc(uint8(output{end})) ;
    end
    axis image ;

    subplot(3,2,6) ;
    if ~isempty(opts.xmask)
        hist(reshape(x(opts.xmask==1),[],1),100);
    else
        hist(x(:),100) ;
    end
    grid on ;
    title('histogram of x') ;

    subplot(3,2,5) ;
    plot(E') ;
    h = legend('recon', 'tv_reg', 'l2_reg', 'tot') ;
    set(h,'color','none') ; grid on ;
    title(sprintf('iter:%d \\lambda_{tv}:%g \\lambda_{l2}:%g rate:%g obj:%s', ...
                  t, opts.lambdaTV, opts.lambdaL2, lr, opts.objective)) ;

   drawnow ;

  end % end if(mod(t-1,25) == 0)
end % end loop over maxNumIterations
if opts.dsp<=0
    output{end+1} = opts.denormalize(x) ;
end

% Compute the features optained using feedforward on the computed inverse
res_nn = res;

clear res;
%res.input = NaN;
res.output = output ;
res.energy = E ;
%res.y0 = y0 ;
%res.y = res_nn(end-1).x ;
%res.opts = opts ;
%res.err = res_nn(end).x / y0_sigma^2 ;

% --------------------------------------------------------------------
function [e, dx] = tv(x,beta,mask)
% dw: increase only boundary weight doesn't help
% which equivalently push the boundary inside by 1-pix
% --------------------------------------------------------------------
if(~exist('beta', 'var'))
  beta = 1; % the power to which the TV norm is raized
end
if(~exist('mask', 'var'));mask=[];end

d1 = x(:,[2:end end],:,:) - x ;
d2 = x([2:end end],:,:,:) - x ;
if ~isempty(mask)
    new_m = imdilate(mask,strel('disk',1))>0;
    d1 = bsxfun(@times,d1, new_m);
    d2 = bsxfun(@times,d2, new_m);
end
v = sqrt(d1.*d1 + d2.*d2).^beta ;
e = sum(sum(sum(sum(v)))) ;
if nargout > 1
  d1_ = (max(v, 1e-5).^(2*(beta/2-1)/beta)) .* d1;
  d2_ = (max(v, 1e-5).^(2*(beta/2-1)/beta)) .* d2;
  % take grad from both end
  %  x_{i,j}-x_{i,j+1}
  %  x_{i,j-1}-x_{i,j}
  d11 = d1_(:,[1 1:end-1],:,:) - d1_ ;
  d22 = d2_([1 1:end-1],:,:,:) - d2_ ;
  d11(:,1,:,:) = - d1_(:,1,:,:) ;
  d22(1,:,:,:) = - d2_(1,:,:,:) ;
  dx = beta*(d11 + d22);
if ~isempty(mask)
    dx = bsxfun(@times,dx,mask);
end
  if(any(isnan(dx)))
  end
end

% --------------------------------------------------------------------
function test_tv()
% --------------------------------------------------------------------
x = randn(5,6,1,1) ;
[e,dr] = tv(x,6) ;
vl_testder(@(x) tv(x,6), x, 1, dr, 1e-3) ;


%==========================================================================
%
%  By Gennady Zilberman, Ben-Gurion University of the Negev
%  genazilberman@yahoo.com
%
%   This software performs Forward Channel simulation of IS-95 standrd
%   system.
%
%   The simulation environment: Static AWGN channel.
%   Link Rate = 9600 KBps
%
%==========================================================================

global Zi Zq Zs show R Gi Gq

clear j;
show = 0;
SD = 0;         % ---- Soft/Hard Decision Receiver

%==========================================================================
% ---------------------------------------- MAIN SIMULATION DEFINITIONS --------------------------------
%==========================================================================
BitRate = 9600;
ChipRate = 1228800;
N = 184;  		         % 9.6 KBps rate -> 184 netto data bits in each 20 msec packet
MFType = 1;		     % Matched filter type - Raised Cosine
R = 5;			            % Analog Signal Simulation rate

% ------------------------ Viterbi Polynom -------------------
G_Vit = [1 1 1 1 0 1 0 1 1; 1 0 1 1 1 0 0 0 1];
K = size(G_Vit, 2); 		% number of cheap per data bit
L = size(G_Vit, 1); 		% number of cheap per data bit

% ------------------------ Walsh Matrix -------------------
WLen = 64;
Walsh = reshape([1;0]*ones(1, WLen/2), WLen , 1);
%Walsh = zeros(WLen ,1);

%------- Spreading PN polynomials ---------
%Gi = [ 1 0 1 0 0 0 1 1 1 0 1 0 0 0 0 1]';
%Gq = [ 1 0 0 1 1 1 0 0 0 1 1 1 1 0 0 1]';

Gi_ind = [15, 13, 9, 8, 7, 5, 0]';
Gq_ind = [15, 12, 11, 10, 6, 5, 4, 3, 0]';

Gi = zeros(16, 1);
Gi(16-Gi_ind) = ones(size(Gi_ind));
Zi = [zeros(length(Gi)-1, 1); 1];     % Initial State for I-Channel PN generator

Gq = zeros(16, 1);
Gq(16-Gq_ind) = ones(size(Gq_ind));
Zq = [zeros(length(Gq)-1, 1); 1]; 	  % Initial State for Q-Channel PN generator

%------- Scrambler Generation Polynom ---------
Gs_ind = [42, 35, 33, 31, 27, 26, 25, 22, 21, 19, 18, 17, 16, 10, 7, 6, 5, 3, 2, 1, 0]';
Gs = zeros(43, 1);
Gs(43-Gs_ind) = ones(size(Gs_ind));
Zs = [zeros(length(Gs)-1, 1); 1];  		% Initial State for Long Sequence Generator

%------- AWGN definitions -------------
EbEc = 10*log10(ChipRate/BitRate); 		% Bit/Chip rate loss
EbEcVit = 10*log10(L);
EbNo = [-2 : 0.5 : 6.5]; 		% measured EbNo range (dB)

%==========================================================================
% ----------------------------------------------- MAIN SIMULATION LOOP ---------------------------------
%==========================================================================
ErrorsB = []; ErrorsC = []; NN = [];
if (SD == 1)
   fprintf('\n SOFT Decision Viterbi Decoder\n\n');
else
   fprintf('\n HARD Decision Viterbi Decoder\n\n');
end

for i=1:length(EbNo)
   fprintf('\nProcessing %1.1f (dB)', EbNo(i));
   iter = 0;	ErrB = 0; ErrC = 0;
   while (ErrB <300) & (iter <150)
      drawnow;

      %----------------------------------- Data Transmit ---------------------------------
      TxData = (randn(N, 1)>0);          %- Gamble the Tx data

      [TxChips, Scrambler] = PacketBuilder(TxData, G_Vit, Gs);		% 19.2 Kcps rate
      [x PN MF] = Modulator(TxChips, MFType, Walsh);  % 1.2288 Mcps rate

      %-------------------------------- AWGN Channel ------------------------------------
      noise = 1/sqrt(2)*sqrt(R/2)*( randn(size(x)) + j*randn(size(x)))*10^(-(EbNo(i) - EbEc)/20);
      r = x+noise;

      %------------------------------------ Receiver ---------------------------------
      RxSD = Demodulator(r, PN, MF, Walsh); 		% Soft Decision   19.2 Kcps
      RxHD = (RxSD>0); 		% Define Hard Decisions received chips
      if (SD)
        % RxSD = sign(RxSD); 		% SOFT Decision = HARD Decision
        % RxSD = sign(RxSD).*(log2(abs(RxSD)+1)); 		% SOFT decision = 3 bits representation
        [RxData Metric]= ReceiverSD(RxSD, G_Vit, Scrambler); 		% Get Received Data 9.6 KBps (Soft Decision)
      else
        [RxData Metric]= ReceiverHD(RxHD, G_Vit, Scrambler); 		% Get Received Data 9.6 KBps (Hard Decision)
      end

      if(show)
         subplot(311); plot(RxSD, '-o'); title('Soft Decisions');
         subplot(312); plot(xor(TxChips, RxHD), '-o'); title('Chip Errors');
         subplot(313); plot(xor(TxData, RxData), '-o');
         title(['Data Bit Errors. Metric = ', num2str(Metric)]);
         pause;
      end

      if(mod(iter, 50)==0)
         fprintf('.');
         save TempResults ErrB ErrC N iter
      end

      ErrB = ErrB + sum(xor(RxData, TxData));  % Data bits Error
      ErrC = ErrC + sum(xor(RxHD, TxChips));  	  % Chip Error (before Viterbi)
      iter = iter+ 1;
   end
   %---------------------------------------- Save the data of current  iteration ---------------------
   ErrorsB = [ErrorsB; ErrB];
   ErrorsC = [ErrorsC; ErrC];
   NN = [NN; N*iter];
   save SimData *
end

%---------------------------------------- Calculate Error's probability ----------------------------
PerrB = ErrorsB./NN;
%PerrB1 = ErrorsB1./NN1;
PerrC = ErrorsC./NN;
Pbpsk= 1/2*erfc(sqrt(10.^(EbNo/10)));
PcVit= 1/2*erfc(sqrt(10.^((EbNo-EbEcVit)/10)));
Pc =   1/2*erfc(sqrt(10.^((EbNo-EbEc)/10)));

%---------------------------------------- Show simulation Results -------- ---------------------
figure;
semilogy(EbNo(1:length(PerrB)), PerrB, 'r-o'); hold on;
%semilogy(EbNo(1:length(PerrB1)), PerrB1, 'k-o'); hold on;
semilogy(EbNo(1:length(PerrC)), PerrC, 'm-o'); grid on;
semilogy(EbNo, Pbpsk, 'b-.x'); xlabel('EbNo (dB)');
%semilogy(EbNo, PcVit, 'k-.x'); ylabel('BER');
semilogy(EbNo, Pc, 'g-.x');

title('Error probability as function of EbNo');

legend('Pb of System (HD)', 'Pb of System (SD)', 'Pc before Viterbi of System', ...
   'Pb of BPSK with no Viterbi (theory)', 'Pc on Receiver (theory)');

legend('Pb of System', 'Pc before Viterbi of System', ...
   'Pb of BPSK with no Viterbi (theory)', 'Pc before Viterbi (theory)', 'Pc on Receiver (theory)');


function f = foldHOG(w)
% Condense HOG features into one orientation histogram.
%   f = foldHOG(w)
%
%   Used for displaying features and filters

% AUTORIGHTS
% -------------------------------------------------------
% Copyright (C) 2011-2012 Ross Girshick
% Copyright (C) 2008, 2009, 2010 Pedro Felzenszwalb, Ross Girshick
% Copyright (C) 2007 Pedro Felzenszwalb, Deva Ramanan
%
% This file is part of the voc-releaseX code
% (http://people.cs.uchicago.edu/~rbg/latent/)
% and is available under the terms of an MIT-like license
% provided in COPYING. Please retain this notice and
% COPYING if you use this file (or a portion of it) in
% your project.
% -------------------------------------------------------

% Return the contrast insensitive orientations
f = w(:,:,19:27);

% f=max(w(:,:,1:9),0)+max(w(:,:,10:18),0)+max(w(:,:,19:27),0);


% demo SIFT using LabelMe toolbox

img = imread('demo1.jpg');
img = imresize(img, .5, 'bilinear');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SIFT parameters:
SIFTparam.grid_spacing = 1; % distance between grid centers
SIFTparam.patch_size = 16; % size of patch from which to compute SIFT descriptor (it has to be a factor of 4)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% CONSTANTS (you can not change this)
w = SIFTparam.patch_size/2; % boundary

% COMPUTE SIFT: the output is a matrix [nrows x ncols x 128]
tic;
SIFT = LMdenseSift(img, '', SIFTparam);
toc

figure
subplot(121)
imshow(img(w:end-w+1,w:end-w+1,:))
title('cropped image')
subplot(122)
showColorSIFT(SIFT)
title('SIFT color coded')


function [S,U] = scat_joint_timefreq(X,cascade)
	Y = concatenate_freq(X);

	S = {};
	U = {};

	for m = 0:length(X)-1
		r = 1;

		S{m+1} = {};
		U{m+1} = {};

		for k = 1:length(Y{m+1}.signal)
			signal = Y{m+1}.signal{k};

			sz_orig = size(signal);
			if numel(sz_orig) == 2
				sz_orig(3) = 1;
			end

			ind = r:r+size(signal,1)-1;

			signal = reshape(signal,[sz_orig(1) sz_orig(2) sz_orig(3)]);

			if m > 0
				% note that here scat is 2d
				[S_fr,U_fr] = scat(signal,cascade);

				% needed for the case of U, are not init by scat
				if ~isfield(U_fr{1}.meta,'bandwidth')
					U_fr{1}.meta.bandwidth = 2*pi*ones(2,1);
				end
				if ~isfield(U_fr{1}.meta,'resolution')
					U_fr{1}.meta.resolution = 0*ones(2,1);
				end
				if ~isfield(U_fr{1}.meta,'j1')
					U_fr{1}.meta.j1 = -1*ones(1,1);
				end
				if ~isfield(U_fr{1}.meta,'j2')
					U_fr{1}.meta.j2 = -1*ones(1,1);
				end
			else
				S_fr = {};

				S_fr{1}.signal = {signal};
				S_fr{1}.meta.bandwidth = 2*pi*ones(2,1);
				S_fr{1}.meta.resolution = 0*ones(2,1);
				S_fr{1}.meta.j1 = -1*ones(1,1);
				S_fr{1}.meta.j2 = -1*ones(1,1);

				U_fr = {};

				U_fr{1}.signal = {signal};
				U_fr{1}.meta.bandwidth = 2*pi*ones(2,1);
				U_fr{1}.meta.resolution = 0*ones(2,1);
				U_fr{1}.meta.j1 = -1*ones(1,1);
				U_fr{1}.meta.j2 = -1*ones(1,1);
			end

			if isempty(S{m+1})
				for mp = 0:length(S_fr)-1
					S{m+1}{mp+1}.signal = {};
					U{m+1}{mp+1}.signal = {};
					rp(mp+1) = 1;
					rb(mp+1) = 1;
				end
			end

			for mp = 0:length(S_fr)-1
				% what do these do?
				%S_fr{mp+1} = unpad_layer_1d(S_fr{mp+1},[size(signal,1), size(signal,2)]);
				%U_fr{mp+1} = unpad_layer_1d(U_fr{mp+1},[size(signal,1), size(signal,2)]);

				for kp = 1:length(S_fr{mp+1}.signal)
					for t = 0:1
						if t == 0
							X_fr = S_fr;
							X = S;
							rc = rp;
						else
							X_fr = U_fr;
							X = U;
							rc = rb;
						end

						nsignal = X_fr{mp+1}.signal{kp};

						j1_count = size(nsignal,1);
						t_count = size(nsignal,2);

						ds = X_fr{mp+1}.meta.resolution(:,kp);

						j1_inds = ind(1:2^ds(1):end);

						nsignal = reshape(nsignal,[j1_count t_count sz_orig(3)]);

						for j1 = 1:j1_count
							X{m+1}{mp+1}.signal{rc(mp+1)} = ...
								reshape(nsignal(j1,:,:),t_count,sz_orig(3));
							X{m+1}{mp+1}.meta.bandwidth(1,rc(mp+1)) = ...
								X_fr{mp+1}.meta.bandwidth(2,kp)/2*pi* ...
								Y{m+1}.meta.bandwidth(j1_inds(j1));
							X{m+1}{mp+1}.meta.resolution(1,rc(mp+1)) = ...
								X_fr{mp+1}.meta.resolution(2,kp)+ ...
								Y{m+1}.meta.resolution(j1_inds(j1));
							X{m+1}{mp+1}.meta.j(:,rc(mp+1)) = ...
								Y{m+1}.meta.j(:,j1_inds(j1));
							X{m+1}{mp+1}.meta.fr_bandwidth(:,rc(mp+1)) = ...
								X_fr{mp+1}.meta.bandwidth(1,kp);
							X{m+1}{mp+1}.meta.fr_resolution(:,rc(mp+1)) = ...
								X_fr{mp+1}.meta.resolution(1,kp);
							X{m+1}{mp+1}.meta.tf_j1(:,rc(mp+1)) = ...
								X_fr{mp+1}.meta.j1(:,kp);
							X{m+1}{mp+1}.meta.tf_j2(:,rc(mp+1)) = ...
								X_fr{mp+1}.meta.j2(:,kp);
							rc(mp+1) = rc(mp+1)+1;
						end

						if t == 0
							S_fr = X_fr;
							S = X;
							rp = rc;
						else
							U_fr = X_fr;
							U = X;
							rb = rc;
						end
					end
				end
			end

			r = r+size(signal,1);
		end
	end

	for m = 0:length(S)-1
		temp = flatten_scat(S{m+1});
		temp = temp{1};
		temp.meta.tf_order = temp.meta.order;
		temp.meta = rmfield(temp.meta,'order');
		S{m+1} = temp;
		temp = flatten_scat(U{m+1});
		temp = temp{1};
		temp.meta.tf_order = temp.meta.order;
		temp.meta = rmfield(temp.meta,'order');
		U{m+1} = temp;
	end
end


function drawAxis3d(varargin)
%DRAWAXIS3D Draw a coordinate system and an origin.
%
%   drawAxis3d
%	Adds 3 cylinders to the current axis, corresponding to the directions
%	of the 3 basis vectors Ox, Oy and Oz.
%	Ox vector is red, Oy vector is green, and Oz vector is blue.
%
%   drawAxis3d(L, R)
%   Specifies the length L and the radius of the cylinders representing the
%   different axes.
%
%   Example
%   drawAxis
%
%   figure;
%   drawAxis(20, 1);
%
%   See also
%   drawAxisCube
%
% ------
% Author: David Legland
% e-mail: david.legland@nantes.inra.fr
% Created: 2007-08-14,    using Matlab 7.4.0.287 (R2007a)
% Copyright 2007 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.

% geometrical data
origin = [0 0 0];
v1 = [1 0 0];
v2 = [0 1 0];
v3 = [0 0 1];

% default parameters
L = 1;
r = L/10;

% extract parameters from input
if ~isempty(varargin)
	L = varargin{1};
end
if length(varargin)>1
	r = varargin{2};
end

% draw 3 cylinders and a ball
hold on;
drawCylinder([origin origin+v1*L r], 16, 'facecolor', 'r', 'edgecolor', 'none');
drawCylinder([origin origin+v2*L r], 16, 'facecolor', 'g', 'edgecolor', 'none');
drawCylinder([origin origin+v3*L r], 16, 'facecolor', 'b', 'edgecolor', 'none');
drawSphere([origin 2*r], 'faceColor', 'black');


function scaled_regions = map_boxes_to_regions_mult_scales(region_params, bboxes, image_size, scales)
% map_boxes_to_regions_mult_scales: given a set of bounding boxes, it
% creates for each of them a region (acording to the region_params provided)
% and it maps each of those regions from the original image with size
% image_size to the appropriate image scale from set of scales given from
% the parameter scales.
%
% 1) region_params: (type struct) the region pooling parameters. Some of
% its fields are:
%   a) sz_conv_standard: (scalar value) the last convolution size
%   b) step_standard: (scalar value) is the stride in pixels of the output
%   of the last convolutional layer of the network where the regions are
%   going to be projected (for VGG16 is equal to 16).
% 2) boxes: a N x 4 array with the bounding box coordinates in the form of
% [x0,y0,x1,y1] (where (x0,y0) is the top-left corner and (x1,y1) the
% bottom left corner)
% 3) image_size: 2 x 1 or 1 x 2 array with the size of the original image
% 4) scales: NS x 1 or 1 x NS array with the image scales that are used. NS
% is the number of images. The i-th value is the size in pixels of the
% smallest dimension of the image in the i-th scale.
%
% OUTPUT:
% 1) scaled_regions: is a N x 9 array with the N output regions. Each region is
% represented by 9 values [scale_id, xo0, yo0, xo1, yo1, xi0, yi0, xi1, yi1]
% that correspond to its outer rectangle [xo0, yo0, xo1, yo1] and its inner
% rectangle [xi0, yi0, xi1, yi1]. scale_id corresponds to the scale id to
% which each region is mapped.
%
% This file is part of the code that implements the following paper:
% Title      : "LocNet: Improving Localization Accuracy for Object Detection"
% Authors    : Spyros Gidaris, Nikos Komodakis
% Institution: Universite Paris Est, Ecole des Ponts ParisTech
% ArXiv link : http://arxiv.org/abs/1511.07763
% code       : https://github.com/gidariss/LocNet
%
% AUTORIGHTS
% --------------------------------------------------------
% Copyright (c) 2016 Spyros Gidaris
%
% Title     : "LocNet: Improving Localization Accuracy for Object Detection"
% ArXiv link: http://arxiv.org/abs/1511.07763
% Licensed under The MIT License [see LICENSE for details]
% ---------------------------------------------------------
regions = create_regions_from_boxes( region_params, bboxes );
[ scaled_regions ] = map_regions_to_mult_scales(region_params, regions, image_size, scales );
end


%This Matlab script can be used to reproduce Figures 4.18-19 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Number of BSs
L = 16;

%Number of UEs per BS
K = 10;

%Define the range of BS antennas
Mrange = 10:10:100;

%Extract maximum number of BS antennas
Mmax = max(Mrange);

%Define the range of pilot reuse factors
fRange = [1 2 4];

%Select the number of setups with random UE locations
nbrOfSetups = 50;

%Select the number of channel realizations per setup
nbrOfRealizations = 500;

%% Propagation parameters

%Communication bandwidth
B = 20e6;

%Total uplink transmit power per UE (mW)
p = 100;

%Total downlink transmit power per UE (mW)
rho = 100;

%Noise figure at the BS (in dB)
noiseFigure = 7;

%Compute noise power
noiseVariancedBm = -174 + 10*log10(B) + noiseFigure;

%Select length of coherence block
tau_c = 200;

%Use the approximation of the Gaussian local scattering model
accuracy = 2;

%Angular standard deviation in the local scattering model (in degrees)
ASDdeg = 10;

%Prepare to save simulation results
sumSE_MR = zeros(length(Mrange),length(fRange),nbrOfSetups);
sumSE_ZF = zeros(length(Mrange),length(fRange),nbrOfSetups);
sumSE_SMMSE = zeros(length(Mrange),length(fRange),nbrOfSetups);
sumSE_RZF = zeros(length(Mrange),length(fRange),nbrOfSetups);
sumSE_MMMSE = zeros(length(Mrange),length(fRange),nbrOfSetups);

%% Go through all setups
for n = 1:nbrOfSetups

    %Output simulation progress
    disp([num2str(n) ' setups out of ' num2str(nbrOfSetups)]);

    %Compute channel statistics for one setup
    [R,channelGaindB] = functionExampleSetup(L,K,Mmax,accuracy,ASDdeg);

    %Compute the normalized average channel gain, where the normalization
    %is based on the noise power
    channelGainOverNoise = channelGaindB - noiseVariancedBm;

    %Go through all number of antennas
    for m = 1:length(Mrange)

        %Output simulation progress
        disp([num2str(m) ' antennas out of ' num2str(length(Mrange))]);

        %Go through all pilot reuse factors
        for s = 1:length(fRange)

            %Extract pilot reuse factor
            f = fRange(s);

            %Generate channel realizations with estimates and estimation
            %error correlation matrices
            [Hhat,C,tau_p,Rscaled,H] = functionChannelEstimates(R(1:Mrange(m),1:Mrange(m),:,:,:),channelGainOverNoise,nbrOfRealizations,Mrange(m),K,L,p,f);

            %Compute SEs with the estimation bound in Theorem 4.6 using
            %Monte Carlo simulations
            [SE_hardening_MR,SE_hardening_RZF,SE_hardening_MMMSE,SE_hardening_ZF,SE_hardening_SMMSE] = functionComputeSE_DL_hardening(H,Hhat,C,Rscaled,tau_c,tau_p,nbrOfRealizations,Mrange(m),K,L,p,rho);

            %Compute SEs with the estimation bound in Theorem 4.9 using
            %Monte Carlo simulations
            [SE_MR,SE_RZF,SE_MMMSE,SE_ZF,SE_SMMSE] = functionComputeSE_DL_estimation(H,Hhat,C,Rscaled,tau_c,tau_p,nbrOfRealizations,Mrange(m),K,L,p,rho);

            %Use the largest of the two bounds
            SE_MR(SE_hardening_MR>SE_MR) = SE_hardening_MR(SE_hardening_MR>SE_MR);
            SE_RZF(SE_hardening_RZF>SE_RZF) = SE_hardening_RZF(SE_hardening_RZF>SE_RZF);
            SE_MMMSE(SE_hardening_MMMSE>SE_MMMSE) = SE_hardening_MMMSE(SE_hardening_MMMSE>SE_MMMSE);
            SE_ZF(SE_hardening_ZF>SE_ZF) = SE_hardening_ZF(SE_hardening_ZF>SE_ZF);
            SE_SMMSE(SE_hardening_SMMSE>SE_SMMSE) = SE_hardening_SMMSE(SE_hardening_SMMSE>SE_SMMSE);

            %Save results
            sumSE_MR(m,s,n) = mean(sum(SE_MR,1));
            sumSE_ZF(m,s,n) = mean(sum(SE_ZF,1));
            sumSE_SMMSE(m,s,n) = mean(sum(SE_SMMSE,1));
            sumSE_RZF(m,s,n) = mean(sum(SE_RZF,1));
            sumSE_MMMSE(m,s,n) = mean(sum(SE_MMMSE,1));

            %Delete large matrices
            clear H Hhat C Rscaled;

        end

    end

    %Delete large matrices
    clear R;

end

%% Plot the simulation results
for s = 1:length(fRange)

    figure(s);
    hold on; box on;

    plot(Mrange,mean(sumSE_MMMSE(:,s,:),3),'rd-','LineWidth',1);
    plot(Mrange,mean(sumSE_SMMSE(:,s,:),3),'b:','LineWidth',1);
    plot(Mrange,mean(sumSE_RZF(:,s,:),3),'k-.','LineWidth',1);
    plot(Mrange,mean(sumSE_ZF(:,s,:),3),'r--','LineWidth',1);
    plot(Mrange,mean(sumSE_MR(:,s,:),3),'bs-','LineWidth',1);

    xlabel('Number of antennas (M)');
    ylabel('Average sum SE [bit/s/Hz/cell]');
    legend('M-MMSE','S-MMSE','RZF','ZF','MR','Location','NorthWest');
    ylim([0 60]);

end


function cm = gold(n)

if nargin == 0,
  n = 64;
end

golds = [255,215,0
  255,215,0
  238,201,0
  205,173,0
  139,117,0]/255;

cm = zeros(n,3);
for i = 1:3,
  cm(:,i) = interp1(linspace(1,n,size(golds,1)),golds(:,i),1:n,'spline');
end
cm = max(0,min(cm,1));


classdef pfSections < ODFSections

  properties
    h1        % the pole figure which is splitted up
    h2        %
    omega
    sR
    referenceField
  end

  properties (Hidden=true)
    maxOmega
  end

  methods

    function oS = pfSections(CS1,CS2,varargin)

      oS = oS@ODFSections(CS1,CS2);

      if isa(CS1,'crystalSymmetry')
        oS.h1 = CS1.cAxisRec; % c*
        oS.h2 = CS1.aAxis; % a
      else
        oS.h1 = vector3d.Z;
        oS.h2 = vector3d.X;
      end

      oS.maxOmega = get_option(varargin,'maxOmega',2*pi / CS1.nfold(oS.h1));
      if angle(oS.h1,-oS.h1) < 1e-2
        oS.sR = CS2.fundamentalSector('upper',varargin{:});
      else
        oS.sR = CS2.fundamentalSector(varargin{:});
      end

      % get sections
      oS.omega = linspace(0,oS.maxOmega,1+get_option(varargin,'sections',6));
      oS.omega(end) = [];
      oS.omega = get_option(varargin,'omega',oS.omega,'double');

      oS.updateTol(oS.omega);

      oS.referenceField = S2VectorField.sigma;
      %oS.referenceField = @(r) S2VectorField.oneSingularity;
      %oS.referenceField = @(r) pfSections.polarField(r);

    end

    function ori = makeGrid(oS,varargin)

      oS.plotGrid = plotS2Grid(oS.sR,varargin{:});
      oS.gridSize = (0:numel(oS.omega)) * length(oS.plotGrid);

      ori = orientation.nan(oS.plotGrid.size(1),oS.plotGrid.size(2),numel(oS.omega),oS.CS1,oS.CS2);
      for iOmega = 1:numel(oS.omega)

        r2 = oS.vectorField(oS.plotGrid,oS.omega(iOmega));
        ori(:,:,iOmega) = reshape(orientation.map(oS.h1,oS.plotGrid,oS.h2,r2),size(oS.plotGrid));

      end

    end

    function ori = quiverGrid(oS,varargin)

      S2G = equispacedS2Grid(oS.sR,varargin{:});

      ori = orientation.nan(S2G.size(1),S2G.size(2),numel(oS.omega),oS.CS1,oS.CS2);
      for iOmega = 1:numel(oS.omega)

        r2 = oS.vectorField(S2G,oS.omega(iOmega));
        ori(:,:,iOmega) = reshape(orientation.map(oS.h1,S2G,oS.h2,r2),size(S2G));

      end
    end

    function n = numSections(oS)
      n = numel(oS.omega);
    end

    function [r,secPos] = project(oS,ori,varargin)

      % maybe this can be done more efficiently
      if ~check_option(varargin,'noSymmetry')
        ori = ori.symmetrise('proper').';
      end

      % determine pole figure position
      r = ori * oS.h1;

      % determine omega angle
      rF = ori * oS.h2;
      vF = vectorField(oS,r);
      omegaSec = angle(vF,rF,r);

      % determine section number
      secPos = oS.secList(omegaSec,oS.omega);

    end

    function ori = iproject(oS,rho,theta,iOmega)
      r1 = vector3d.byPolar(theta,rho);
      r2 = oS.vectorField(r1,oS.omega(iOmega));

      ori = orientation.map(oS.h1,r1,oS.h2,r2);
    end

    function h = plotSection(oS,ax,sec,v,data,varargin)

      % plot data
      h = plot(v,data{:},oS.sR,'TR',[int2str(oS.omega(sec)./degree),'^\circ'],...
        'parent',ax,varargin{:},'doNotDraw');

      wasHold = ishold(ax);
      hold(ax,'on');
      r = equispacedS2Grid(oS.sR,'resolution',15*degree);
      vF = oS.vectorField(r,oS.omega(sec));
      quiver(r,vF,'parent',ax,'doNotDraw','color',0.7*[1 1 1],'HitTest','off');
      if ~wasHold, hold(ax,'off'); end

    end

    function h = quiverSection(oS,ax,sec,v,data,varargin)

      % translate rotational data into tangential data
      if iscell(data) && isa(data{1},'quaternion')
        [v2,sec2] = project(oS,data{1},'noSymmetry');
        data{1} = v2 - v;
        data{1}(sec2 ~= sec)=NaN;
      end
      if check_option(varargin,'normalize'), data{1} = normalize(data{1}); end

      % plot data
      h = quiver(v,data{:},oS.sR,'TR',[int2str(oS.omega(sec)./degree),'^\circ'],...
        'parent',ax,varargin{:},'doNotDraw');

    end

    function vF = vectorField(oS,r,omega)

      vF = oS.referenceField.eval(r);

      if nargin == 3
        vF = rotation.byAxisAngle(r,omega) .* vF;
      end

    end

  end

end

% testing code
% r = equispacedS2Grid('resolution',20*degree)
% quiver(r,vector3d(vF))


function dxx = FTSystemWrapper(t,x,Params,K)
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% FTSystemWrapper: System Dynamics with external integrators for learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Create local copies of the parameters
[F,G,Q,R] = deal(Params.F,Params.G,Params.Q,Params.R);

[e1,e2] = ExplNoise(t);
xm = BasisPlanMono(x(1),x(2));
u = K*xm + [e1;e2];

dx = F*xm + G*u;
dphi = [BasisQuadPlantMono(x(1),x(2));2*e1*R*xm;2*e2*R*xm];
dQ = xm'*(Q+K'*R*K)*xm;

dxx=[dx;dphi;dQ]; %size 2+ 34 +1 = 37
end


function cS = monazite

%
cs_Mnz = crystalSymmetry('2/m',[0.967 1 0.921], [90, 103.4, 90]*degree);
N = Miller({1,0,0},{0,1,0},{1,1,0},{0,1,1},{1,1,-1},{0,2,1},{0,1,2},{1,0,-1},{1,0,1},{3,-1,-1},{2,-1,0},{0,-1,-1},{1,-2,-1},cs_Mnz);
dist = [3.6, 4.95, 6.5, 10, 8.2, 10.65, 11.1, 6.75, 7.45, 14.65, 9.9, 6.15, 12.2];
cS = crystalShape(N./dist);


function output = ...
  ConvertToObservationProbatility(f0_structure, frame_shift)
% Convert wavelet filter bank output to ovserbation probability map
%
%   output = ConvertToObservationProbatility(f0_structure, frame_shift)
%
% Arguments
%
% f0_structure: structure with the following fields.
%     Note that only used fieds are listed below.
%   temporal_positions: vector: center location of the analysis window.
%     represented in terms of second
%   sampling_frequency: scalar: sampling frequency (Hz)
%   center_frequency_list: vector: center frequency of each wavelet filter
%   (Hz)
%   inst_freq_map: matrix: instantaneous frequency of each filter output
%   (Hz)
%   residual_map: matrix: relative level of random component (power ratio)
%   amplitude_map: matrix: amplitude of filter output (absolute value)
% frame_shift: optional variable, scalar, real value in second
%   Interval between adjacent frame center positions
%
% Return value
%
% output: structure with the following fields.
%   temporal_positions: vector: center location of the analysis window.
%   frame_shift: scalar: interval between frame locations
%   inst_freq_map: matrix: instantaneous frequency of each channel (Hz)
%   residual_map: relative resitual of each channel
%           (relative power, calibrated for Nuttall-11 window.)
%   observation_probability_map: matrix: probability time series of each
%           channel
%   amplitude_map: matrix: amplitude of filter output (absolute value)
%   channels_in_octave: scalar: number of wavelet channels in one octave
%   center_frequency_list: vector: list of center frequencies of wavelet
%           filters (Hz)

% Copyright 2016 Google Inc. All Rights Reserved
% Author: hidekik@google.com (Hideki Kawahara)

if nargin == 1, frame_shift = 0.005; end;

startTic = tic;
calibration_constant = 64; % This is for NuttallWindows(~, 11)
time_axis = f0_structure.temporal_positions;
fs = f0_structure.sampling_frequency;
temporal_positions = (0:frame_shift:time_axis(end))';
frame_index = ...
  min(f0_structure.sample_length,max(1, round(fs * temporal_positions)));
number_of_channels = length(f0_structure.center_frequency_list);
inst_freq_map = f0_structure.inst_freq_map(:, frame_index);
residual_map = f0_structure.residual_map(:, frame_index);
amplitude_map = f0_structure.amplitude_map(:, frame_index);
tmp_variance_map = ...
  ConvertPowerToDecibel(residual_map) + calibration_constant;
snr_map = 10.0 .^ (tmp_variance_map / 20);
best_weight = snr_map * 0;
for ii = 1 : length(temporal_positions)
  best_weight(:, ii) = GetBestMixingWeight(snr_map(:, ii));
end;
observation_probability_map = tmp_variance_map * 0;
center_frequency_list = f0_structure.center_frequency_list;
tmp_half_bw = sqrt(center_frequency_list(2) / center_frequency_list(1));
fixed_best_weight = best_weight .* snr_map;
fc_in_cent_U = 1200 * log2(center_frequency_list * tmp_half_bw);
fc_in_cent_L = 1200 * log2(center_frequency_list / tmp_half_bw);
for ii = 1:length(temporal_positions)
  for kk = 1 : number_of_channels
    fqi_cent = 1200 * log2(max(20, inst_freq_map(kk, ii)));
    tmpU = erf((fc_in_cent_U - fqi_cent) / snr_map(kk, ii) / sqrt(2));
    tmpL = erf((fc_in_cent_L - fqi_cent) / snr_map(kk, ii) / sqrt(2));
    tmpProb = (tmpU - tmpL) / sum(tmpU - tmpL);
    observation_probability_map(:, ii) = ...
      observation_probability_map(:, ii) ...
      + fixed_best_weight(kk, ii) * tmpProb(:);
  end;
  observation_probability_map(:, ii) = observation_probability_map(:, ii) ...
    / sum(observation_probability_map(:, ii));
end;
output = struct('temporal_positions', temporal_positions, ...
                'frame_shift', frame_shift', ...
                'inst_freq_map', inst_freq_map, ...
                'residual_map', residual_map, ...
                'observation_probability_map', observation_probability_map, ...
                'amplitude_map', amplitude_map, ...
                'channels_in_octave', f0_structure.channels_in_octave', ...
                'center_frequency_list', center_frequency_list);
end


function midrad(c)
%MIDRAD       Display of interval hessians in midrad notation
%
%   midrad(c)
%

% written  04/04/04     S.M. Rump
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 02/11/06     S.M. Rump  SparseInfNanFlag removed
% modified 06/04/09     S.M. Rump  Comment
% modified 08/26/12     S.M. Rump  global variables removed
%

  if nargin<2
    name = inputname(1);
    if isempty(name)                    % happens for display(hessianinit(random))
      name = 'ans';
    end
  end

  if isa(c.x,'intval')
    display_gen(c,name,'midrad')
  else
    display_gen(c,name,'display_')
  end


function m = rvecrep(v,c)

% RVECREP  Row vector replicate
%
%   M = rvecrep(V, C) Replicates a 1xN dimensional row vector V, C times to generate a
%   CxN dimensional matrix M.
%
%   See also
%   CVECREP, REPMAT

%   Copyright (c) Oregon Health & Science University (2006)
%
%   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for
%   academic use only (see included license file) and can be obtained from
%   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the
%   software should contact rebel@csee.ogi.edu for commercial licensing information.
%
%   See LICENSE (which should be part of the main toolkit distribution) for more
%   detail.

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

if isempty(v)

    m = zeros(c,0);

else

    m = v(ones(1,c),:);

end


function x = emailFeatures(word_indices)
%EMAILFEATURES takes in a word_indices vector and produces a feature vector
%from the word indices
%   x = EMAILFEATURES(word_indices) takes in a word_indices vector and
%   produces a feature vector from the word indices.

% Total number of words in the dictionary
n = 1899;

% You need to return the following variables correctly.
x = zeros(n, 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Fill in this function to return a feature vector for the
%               given email (word_indices). To help make it easier to
%               process the emails, we have have already pre-processed each
%               email and converted each word in the email into an index in
%               a fixed dictionary (of 1899 words). The variable
%               word_indices contains the list of indices of the words
%               which occur in one email.
%
%               Concretely, if an email has the text:
%
%                  The quick brown fox jumped over the lazy dog.
%
%               Then, the word_indices vector for this text might look
%               like:
%
%                   60  100   33   44   10     53  60  58   5
%
%               where, we have mapped each word onto a number, for example:
%
%                   the   -- 60
%                   quick -- 100
%                   ...
%
%              (note: the above numbers are just an example and are not the
%               actual mappings).
%
%              Your task is take one such word_indices vector and construct
%              a binary feature vector that indicates whether a particular
%              word occurs in the email. That is, x(i) = 1 when word i
%              is present in the email. Concretely, if the word 'the' (say,
%              index 60) appears in the email, then x(60) = 1. The feature
%              vector should look like:
%
%              x = [ 0 0 0 0 1 0 0 0 ... 0 0 0 0 1 ... 0 0 0 1 0 ..];
%
%

for i=1:length(word_indices)
    x(word_indices(i)) = 1;
end
% =========================================================================

end


%% RRT*FN Toolbox
% *RRT*FN Toolbox* for solving path/motion planning problems.
% Toolbox provides *RRT*, *RRT** and *RRT*FN* for solving various problems
% (e.g. 2D mobile robot, planar redundant manipulator)
%
% Sampling-based motion planning algorithms got a lot of research attention
% in past decade or so. The main reason of the popularity is an ability
% to solve relatively easy motion planning problems of 2D mobile robot
% as well as hard high dimensional problems both.
%
% *Rapidly-Exporing Random Tree (RRT)* is sampling-based algorithm, solves
% the problem of motion and path planning providing feasible solutions
% however it does not take into account optimality of the solution.
%
% <<rrt_pseudocode.png>>
%
% *RRT** is probabilistically optimal variant of RRT which converges to the
% optimal solution asymtotically. Due to the fact that RRT* takes cost of
% the path into consideration, the computational diffuculty increases with
% each node added to tree, a good example of this problem is finding
% neighbors.
%
% <<rrt_star_pseudocode.png>>
%
% <<rrt_star_pseudocode_1.png>>
%
% *RRT*FN (Fixed Nodes)* is a memory efficient variant of RRT*. It
% inherents all the optimization features of RRT*, but in contrast using
% limited number of nodes i.e. memory is limited.
%
% <<rrt_star_fn_pseudocode.png>>
%
%%
%
% <<rrts_comparison.png>>
%%
%

% Please do not change anything in rrt.m,
% rrt_star.m and rrt_star_fn.m files unless it is a bug.
Everything
% you want to add (e.g. new function or modifications of a model)
% please do it creating/editing classes for models.
% e.g. FNSimple2D.m or
% FNRedundantManipulator.m
%

%


%
%      This HDR Toolbox demo creates an LDR stack from an HDR image:
%	   1) Read a stack of LDR images
%	   2) Read exposure values from the EXIF
%	   3) Estimate the Camera Response Function (CRF)
%	   4) Build the radiance map using the stack and stack_exposure
%	   5) Save the radiance map in .hdr format
%	   6) Show the tone mapped version of the radiance map
%
%       Author: Francesco Banterle
%       Copyright 2016 (c)
%

clear all;

img = hdrimread('Bottles_Small.hdr');

name_folder_h = 'demos/output/hdr_to_stack_histogram';
name_folder_u = 'demos/output/hdr_to_stack_uniform';
name_folder_s = 'demos/output/hdr_to_stack_selected';

format = 'jpg';

%
% Histogram
%

mkdir(name_folder_h);

[stack, stack_exposure] = CreateLDRStackFromHDR( img, 2, 'histogram', 'gamma', 2.2);

for i=1:size(stack, 4)
    name_out = [name_folder_h, '/exp_', num2str(stack_exposure(i)), '.', format];
    imwrite(stack(:,:,:,i), name_out);
end

%
% Uniform
%

mkdir(name_folder_u);

[stack, stack_exposure] = CreateLDRStackFromHDR( img, 2, 'uniform', 'gamma', 2.2);

for i=1:size(stack, 4)
    name_out = [name_folder_u, '/exp_', num2str(stack_exposure(i)), '.', format];
    imwrite(stack(:,:,:,i), name_out);
end

%
% Selected
%

mkdir(name_folder_s);

selected_exps = [-1.0 0.0 1.0];

[stack, stack_exposure] = CreateLDRStackFromHDR( img, selected_exps, 'selected', 'gamma', 2.2);

for i=1:size(stack, 4)
    name_out = [name_folder_s, '/exp_', num2str(stack_exposure(i)), '.', format];
    imwrite(stack(:,:,:,i), name_out);
end


%compute dtailheadmag
function [data,units]=compute_dtailheadmag(trx,n)

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
dtailheadmag=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    dtailheadmag{1,i}=(trx(larva).tailheadmag(2:end)-trx(larva).tailheadmag(1:end-1))./trx(larva).dt;
end

units=parseunits('mm/s');
data=dtailheadmag;


function GetPlotRange(handles)
if isempty(handles.CurrentData), return; end
Current = GetCurrent(handles);
values=Current(:); values(isinf(values))=[]; values(isnan(values))=[];

if length(unique(values))>20 % it is a mask?
    values(~values)=[];
    [Min, Max] = range_outlier(values);
else
    Min=min(values);
    Max=max(values);
end

if (abs(Min - Max)<1e-3)
    Max = Max + 1;
end
if (Min > Max)
    temp = Min;
    Min = Max;
    Max = temp;
end

if (Min < 0)
    set(handles.MinSlider, 'Min',    1.5*Min);
else
    set(handles.MinSlider, 'Min',    0.5*Min);
end

if (Max < 0)
    set(handles.MaxSlider, 'Max',    0.5*Max);
else
    set(handles.MaxSlider, 'Max',    1.5*Max);
end
set(handles.MinSlider, 'Max',    Max);
set(handles.MaxSlider, 'Min',    Min);
set(handles.MinValue,  'String', Min);
set(handles.MaxValue,  'String', Max);
set(handles.MinSlider, 'Value',  Min);
set(handles.MaxSlider, 'Value',  Max);
guidata(findobj('Name','qMRLab'), handles);


%ControlCentor
% when you test the code,please correct the directory in next command
load('G:\K2\Sample.mat');

% Sample is a variable that saves our training database.
LGObj = ConstructLGObj( Sample); % construct an object

Order = [3 4 1 2 5 8 7 10 9 6]; % Order is the ordering of the input in K2 algorithm

u = 4; % u is the maximum edges of node in output graph.

[ DAG,K2Score ] = k2( LGObj,Order,u )
h = view(biograph( DAG ))


function [ mean_llr ] = get_bpsk_llr_for_capacity( const_capacity )
%UNTITLED2 Summary of this function goes here
%   Detailed explanation goes here

mean_llr = 0 * const_capacity;

load('bpsk_cap.mat');

for i = 1:size(const_capacity,1)
    for j = 1:size(const_capacity,2)
        for k = 1:size(const_capacity,3)
            for snr_index = 1 : length(snr_vec_db)
                if capacity(snr_index) >= const_capacity(i, j, k)
                    break;
                end
            end
            mean_llr(i, j, k) = 4 * 10^(snr_vec_db(snr_index)/10);
        end
    end
end
end


function K = linear(kern,dat1,dat2,ind1,ind2,kerParam),

K = get_x(dat2,ind2)*get_x(dat1,ind1)';


function DEM_demo_PEB
% DEM demo for a hierarchical linear model (MFX).  This inversion is
% cross-validated with restricted maximum likelihood using and parametric
% empirical Bayes (spm_PEB). It uses a simple two level model that embodies
% empirical shrinkage priors on the first-level parameters (c.f.,
% DEM_demo_GLM, with no empirical priors)

% MFX design
%==========================================================================
X1    = kron(eye(4),kron(ones(2,1),eye(2)));
X2    = kron(ones(4,1),eye(2));

% specify parameters
%--------------------------------------------------------------------------
N     = 1;                                      % length of data sequence
h     = {log(16) log(8)};                       % precisions

% generate data
%--------------------------------------------------------------------------
DEM   = spm_DEM_generate(spm_DEM_M('HLM',X1,X2),N,{},h);

% DEM estimation
%==========================================================================
DEM.M(1).E.nE = 16;

DEM   = spm_DEM(DEM);
qU    = DEM.qU;
qP    = DEM.qP;
qH    = DEM.qH;

% compare real and estimated factor and causes (using ReML and PEB)
%==========================================================================
P            = cell(1,2);
Q1           = DEM.M(1).Q{1};
Q2           = DEM.M(2).Q{1};
[Ce,hr,W,Fr] = spm_reml_sc(DEM.Y*DEM.Y',X1*X2,{Q1; X1*Q2*X1'},N);
P{1}.X       = X1;
P{1}.C       = {Q1};
P{2}.X       = X2;
P{2}.C       = {Q2};
[C,P,Fp]     = spm_PEB(DEM.Y,P);

% graphics for variance component estimators
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 1');

subplot(3,1,1)
qh   = spm_vec(qH.h);
bar([exp(-[h{1}; h{2}]) [exp(-qh)] [C{1}.h; C{2}.h]])
legend({'true' 'DEM' 'ReML'})
title({'ReML estimators';'Parametric Empirical Bayes'},'FontSize',16)
c     = sqrt(diag(qH.C))*1.64;
for i = 1:length(h)
    line([i i],exp(-([-c(i) c(i)] + qh(i))),'color',[1 0 0],'LineWidth',2)
end
axis square
grid on

% check estimators
%--------------------------------------------------------------------------
subplot(3,2,3)
bar(qU.v{2})
axis square; title('DEM level-1','FontSize',16)
a = axis;
subplot(3,2,4)
bar(C{2}.E)
axis square; title('VB level-1','FontSize',16)
axis(a)

subplot(3,2,5)
bar(qU.v{3})
axis square; title('DEM level-2','FontSize',16)
a = axis;
subplot(3,2,6)
bar(C{3}.E)
axis square; title('VB level-2','FontSize',16)
axis(a)


function p = real(p);
%REAL         Real part of (interval) polynomial
%
%   r = real(p)
%

% written  08/28/00     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  p.c = real(p.c);
  p = normalize(p);


function out=bsxfun(f,x,y)
%BSXFUN

dim1 = size(x);
dim2 = size(y);

X = x;
Y = y;

for i = 1:length(dim1)
    if dim1(i) < dim2(i)
        d = ones(1,length(dim1));
        d(i) = dim2(i);
        X = repmat(X,d);
    elseif dim1(i) > dim2(i)
        d = ones(1,length(dim1));
        d(i) = dim1(i);
        Y = repmat(Y,d);
    end
end

% And compute
out = f(X,Y);


function cadj=ref_spreadadj(coef);
%REF_SPREADADJ  Symbol of adjoint preading function.
%   Usage: cadj=ref_spreadadj(c);
%
%   cadj=SPREADADJ(c) will compute the symbol cadj of the spreading
%   operator that is the adjoint of the spreading operator with symbol c.
%
%   The algorithm converts the symbol to the matrix representation,
%   adjoints its, and finds its spreading function.

L=size(coef,1);

T=tfmat('spread',coef);
cadj=spreadfun(T');


function k = gaussianwhiteKernDiagCompute(kern, x)

% GAUSSIANWHITEKERNDIAGCOMPUTE Compute diagonal of gaussian white kernel.
% FORMAT
% DESC computes the diagonal of the kernel matrix for the gaussian white kernel
% given a design matrix of inputs.
% RETURN K : a vector containing the diagonal of the kernel matrix computed
% at the given points.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG X : input data matrix in the form of a design matrix.
%
% SEEALSO : gaussianwhiteKernParamInit, kernDiagCompute, kernCreate,
% gaussianwhiteKernCompute
%
% COPYRIGHT : Mauricio A. Alvarez and Neil D. Lawrence, 2008
%
% MODIFICATIONS : Mauricio A. Alvarez, 2009.

% KERN

k = kern.sigma2Noise*ones(size(x,1),1);


% CVX: Built-in operators and functions supported in CVX models.
%
%    The following operators and functions are included with MATLAB but
%    have either been verified to work properly within CVX models or have
%    been extended with additional code to do so. In many cases, typing
%       help cvx/<func>
%    where <func> is one of the names listed below will provide specific
%    help on the proper use of that item in CVX models---including any
%    restrictions imposed by the DCP and DGP rulesets.
%
%    For a list of new functions created specifically for CVX, type
%    "help cvx/functions".
%
% Computational operators:
%    plus (+), uplus (unary +), minus (-), uminus (unary -), times (.*),
%    mtimes (*), ldivide (.\), mldivide (\), rdivide (./), mrdivide (/),
%    power (.^), mpower (^), subsref/subsasgn/end (subscripting),
%    transpose (.'), ctranspose (')
% Relational operators:
%    eq (==), ge (>=), gt (>), le (<=), lt(<), ne (~=).
% Linear/affine functions:
%    blkdiag, cat, conj, conv, cumsum, diag, dot, find, flipdim, fliplr,
%    flipud, hankel, horzcat, imag, ipermute, kron, permute, polyval, real,
%    repmat, reshape, rot90, sparse, sum, toeplitz, tril, triu, vertcat
% Nonlinear functions:
%    abs, max, min, norm, prod, sqrt
% Query functions:
%    disp/display, end, isempty, isequal, length, isreal, ndims, nnz,
%    numel, size, spy

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function  gradH = lfmwhiteComputeGradThetaH2(gamma1, gamma2, t1, t2, ...
    gradTheta, isStationary1, isStationary2)

% LFMWHITECOMPUTEGRADTHETAH2 computes a portion of the LFM-WHITE kernel's gradient w.r.t. theta.
% FORMAT
% DESC Helper function for computing part of the gradient of
% the LFM-WHITE kernel w.r.t. to a generic parameter theta (mass, spring or
% damper). Used for obtaining the gradients w.r.t. parameters related to
% the second argument (gamma2).
% ARG gamma1 : Gamma value for first system.
% ARG gamma2 : Gamma value for second system.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG gradTheta : gradient of gamma w.r.t. the generic parameter theta.
% ARG isStationary1: indicates whether the stationary version of the first
% kernel is used (TRUE) or not (FALSE). Set to FALSE by default.
% ARG isStationary2: indicates whether the stationary version of the second
% kernel is used (TRUE) or not (FALSE). Set to FALSE by default.
% RETURN h : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : David Luengo, 2009
%
% SEEALSO : lfmwhiteKernParamInit, lfmwhiteXlfmwhiteKernGradient,
% lfmwhiteComputeH, lfmwhiteComputeGradThetaH1

% KERN

if size(t1, 2) > 1 | size(t2, 2) > 1
  error('Input can only have one column');
end

if nargin < 6
    isStationary1 = false;
end
if nargin < 7
    isStationary2 = false;
end

T1 = repmat(t1, 1, size(t2, 1));
T2 = repmat(t2.', size(t1, 1), 1);
deltaT = T1 - T2;
indT = deltaT >= 0;
psi = gamma1 * (1-indT) + gamma2 * indT;
gradH = -(lfmwhiteComputeH(gamma1, gamma2, t1, t2, isStationary1, isStationary2) ...
    + abs(deltaT) .* exp(-psi .* abs(deltaT)) .* indT);
if ((isStationary1 == false) | (isStationary2 == false))
    gradH = gradH + T1 .* exp(-(gamma2 * T1 * double(isStationary2 == false) ...
        + gamma1 * T2 * double(isStationary1 == false)));
end
gradH = gradH * gradTheta / (gamma1 + gamma2);


classdef DateTime
    % This class enclapsulates date time value but behaves in a very
    % similar way as typical array of timestamps in datenum format
  %======================================================================
    %{
		Copyright (c) 2011
		This program is a result of a joined cooperation of Energocentrum
		PLUS, s.r.o. and Czech Technical University (CTU) in Prague.
        The program is maintained by Energocentrum PLUS, s.r.o. and
        licensed under the terms of MIT license. Full text of the license
        is included in the program release.

        Author(s):
		Jiri Cigler, Dept. of Control Engineering, CTU Prague & Automatic Control Laboratory, ETH Zurich
		Jan  Siroky, Energocentrum PLUS s.r.o.

        Implementation and Revisions:

        Auth  Date        Description of change
        ----  ---------   -------------------------------------------------
        jc    01-Mar-11   First implementation
        jc    30-Sep-11   Added function colon
        jc    07-Jan-12   Added functions addtodate,datevec,weekday
    %}
    %======================================================================

    % TODO

%ate and Time Operations
%addtodateModify date number by fieldcalendarCalendar for specified monthclockCurrent time as date vectorcputimeElapsed CPU timedateCurrent date stringdatenumConvert date and time to serial date numberdatestrConvert date and time to string formatdatevecConvert date and time to vector of componentseomdayLast day of monthetimeTime elapsed between date vectorsnowCurrent date and timeweekdayDay of wee

    properties
        serialDate
    end
    methods
        function this = DateTime(varargin)
            if numel(varargin)==1 && isa(varargin{1},'java.util.Date')
                    df = java.text.SimpleDateFormat( 'yyyy-MM-dd HH:mm:ss' );
                    tz = java.util.TimeZone.getTimeZone ('UTC');
                    df.setTimeZone( tz );
                    this.serialDate=datenum(char(df.format(varargin{1})));
                    %this.serialDate = datenum(char(varargin{1}.toString)) - varargin{1}.getTimezoneOffset/60/24;
                    %disp ( [ char(varargin{1}.toGMTString), '---' char(varargin{1}.toString ), '---', char(varargin{1}.toLocaleString )]);
                    %if (varargin{1}.getTimezoneOffset)~=-120
                    %    disp(1);
                    %end
            else
                this.serialDate=datenum(varargin{:});
            end
        end
        function this = plus(this,val)
            o =@plus;
            this = doFun(this,o,val);
        end
        function this = minus(this,val)
            o =@minus;
            this = doFun(this,o,val);
        end
        function this = times(this,val)
            o =@times;
            this = doFun(this,o,val);
        end

        function this = mtimes(this,val)
            o =@mtimes;
            this = doFun(this,o,val);
        end

        function this = mrdivide(this,val)
            o =@mrdivide;
            this = doFun(this,o,val);
        end

        function this = rdivide(this,val)
            o =@rdivide;
            this = doFun(this,o,val);
        end

        function this = horzcat(this,varargin)
            %this.serialDate = [this.serialDate, n.serialDate];
            for i=1:numel(varargin)
                this.serialDate = [this.serialDate, varargin{i}.serialDate];
            end
        end

        function out = colon(this,step,to)
            vect = [double(this):double(step):double(to)]';

            out =DateTime(vect);
        end

        function this = vertcat(this,varargin)
            for i=1:numel(varargin)
                this.serialDate = [this.serialDate; varargin{i}.serialDate];
            end
        end

        function this = ctranspose(this)
            this.serialDate = this.serialDate';
        end

        function this = transpose(this)
            this.serialDate = this.serialDate';
        end
        function  disp(this)
            disp([this.serialDate])
        end
        function out = double(this)
            out = this.serialDate;
        end
        function out = length(this)
            out = length(this.serialDate);
        end

        function out = size(this,varargin)
            out = size(this.serialDate,varargin{:});
        end

        function out = numel(this)
            out = numel(this.serialDate);
        end
        function out = isreal(this)
            out = isreal(this.serialDate);
        end
        function out = isnan(this)
            out = isnan(this.serialDate);
        end
        function out = isfinite(this)
            out = isfinite(this.serialDate);
        end

        function out = le(this,B)
            if isa(B,'DateTime')
                out = le(this.serialDate,B.serialDate);
            else
                out = le(this.serialDate,B);
            end
        end

        function out = lt(this,B)
            fun=@lt;
            if isa(B,'DateTime')
                out = fun(this.serialDate,B.serialDate);
            else
                out = fun(this.serialDate,B);
            end
        end
        function out = gt(this,B)
            fun=@gt;
            if isa(B,'DateTime')
                out = fun(this.serialDate,B.serialDate);
            else
                out = fun(this.serialDate,B);
            end
        end
        function out = eq(this,B)
            fun=@eq;
            if isa(B,'DateTime')
                out = fun(this.serialDate,B.serialDate);
            else
                out = fun(this.serialDate,B);
            end
        end
        function out = diff(this)
            out = diff(this.serialDate);
        end

        function out = norm(this,varargin)
            out = norm(this.serialDate,varargin{:});
        end

        function [this k] = sort(this,varargin)
            [this.serialDate k] = sort(this.serialDate,varargin{:});
        end

        function this = subsref(this,S)
            if isa(S.subs{1},'DateTime')
                S.subs{1}=double(S.subs{1});
            end

            this.serialDate =  subsref(this.serialDate,S);
        end

        function idx = subsindex(this)
            idx = double(this)-1;
        end

        function endidx = end(this,k,n)
            if size(this.serialDate,1)==1 || size(this.serialDate,2)==1
                endidx=numel(this.serialDate);
            else
                endidx = size(this.serialDate,k);
            end
        end

        function this = subsasgn(this, S, B)
            if not(isa(B,'DateTime'))
                B=DateTime(B);
            end

            this.serialDate =subsasgn(this.serialDate, S, B);
        end

        function res = bsxfun(fun,A,B)
            res = fun(A,B);
        end

        function out =superiorfloat (x,y,xi)
            if isa(x,'DateTime') && isa(xi,'DateTime')
                out = superiorfloat(x.serialDate,y,xi.serialDate);
            elseif isa(x,'DateTime') && not(isa(xi,'DateTime'))
                out = superiorfloat(x.serialDate,y,xi);
            elseif not(isa(x,'DateTime')) && isa(xi,'DateTime')
                out = superiorfloat(x,y,xi.serialDate);
            else
                out = superiorfloat(x,y,xi);
            end
        end

        function this = floor(this)
            this.serialDate = floor(this.serialDate);
        end
        function this = max(this,varargin)
            this.serialDate = max(this.serialDate,varargin{:});
        end
        function this = min(this,varargin)
            this.serialDate = min(this.serialDate,varargin{:});
        end
        function out = datestr(this,varargin)
            out = datestr(this.serialDate,varargin{:});
        end

        function out = addtodate(this,varargin)
            out = addtodate(this.serialDate,varargin{:});
        end
        function varargout= datevec(this,varargin)
            nout = nargout;
            if nout <=1
                varargout{1} = datevec(this.serialDate,varargin{:});
            elseif nout ==2
                [varargout{1} varargout{2}] = datevec(this.serialDate,varargin{:});

            elseif nout ==3
                [varargout{1} varargout{2} varargout{3}] = datevec(this.serialDate,varargin{:});

            elseif nout ==4
                [varargout{1} varargout{2} varargout{3} varargout{4}] = datevec(this.serialDate,varargin{:});

            elseif nout ==5
                [varargout{1} varargout{2} varargout{3} varargout{4} varargout{5} ] = datevec(this.serialDate,varargin{:});

            elseif nout ==6
                [varargout{1} varargout{2} varargout{3} varargout{4} varargout{5} varargout{6} ] = datevec(this.serialDate,varargin{:});
            else
                error('Unknown function call');
            end
        end

    end

    methods (Access = private)
        function this = doFun (this,o, val)
            if isa(val,'DateTime') && isa(this,'DateTime')
                this.serialDate=o(this.serialDate, val.serialDate);
            elseif isa(val,'DateTime') && not(isa(this,'DateTime'))
                val.serialDate=o(this, val.serialDate);
                this = val;
            elseif not(isa(val,'DateTime')) && (isa(this,'DateTime'))
                this.serialDate=o(this.serialDate, val);
            else
                this.serialDate=DateTime(o(this, val));
            end
        end

    end

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_KUKA_KR5_ARC(robot, T)
%   Solves the inverse kinematic problem for the KUKA KR5 ARC robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC__KUKA_KR5_ARC returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('KUKA', 'KR160_R1570_nanoC');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function q = inversekinematic_kuka_kr160_r1570_nanoC(robot, T)

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

% %Evaluate the parameters
% theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
L6=abs(d(6));

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' + L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);
%the other possible solution is q1 + pi
q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);
%solver for q3 for both cases
q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];
%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solucion_muneca_esferica(robot, q(:,i), T, 1,'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solucion_muneca_esferica(robot, q(:,i), T, -1, 'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));
A2 = abs(a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%The inverse kinematic problem can be solved as in the IRB 140 (for example)

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2)));

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));

A2 = abs(a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%the angle phi is fixed
phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

eta = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = phi - eta;
q3(2) = -2*pi +( phi + eta); %%%?????????????????????????????????????????

function q = solucion_muneca_esferica(robot, q, T, wrist, method)

switch method

     %algebraic solution
    case 'algebraic'
        T01=dh(robot, q, 1);
        T12=dh(robot, q, 2);
        T23=dh(robot, q, 3);

        Q=inv(T23)*inv(T12)*inv(T01)*T;

        %detect the degenerate case when q(5)=0, this leads to zeros
        % in Q13, Q23, Q31 and Q32 and Q33=1
        thresh=1e-12;
        %detect if q(5)==0
        % this happens when cos(q5) in the matrix Q is close to 1
        if abs(Q(3,3)-1)>thresh
            %normal solution
            if wrist==1 %wrist up
                q(4)=atan2(-Q(2,3),-Q(1,3));
                q(6)=atan2(-Q(3,2),Q(3,1));
                %q(5)=atan2(-Q(3,2)/sin(q(6)),Q(3,3));
            else %wrist down
                q(4)=atan2(-Q(2,3),-Q(1,3))-pi;
                q(6)=atan2(-Q(3,2),Q(3,1))+pi;
                %q(5)=atan2(-Q(3,2)/sin(q(6)),Q(3,3));
            end
            if abs(cos(q(6)+q(4)))>thresh
                cq5=(Q(1,1)+Q(2,2))/cos(q(4)+q(6))-1;
            end
            if abs(sin(q(6)+q(4)))>thresh
                cq5=(-Q(1,2)+Q(2,1))/sin(q(4)+q(6))-1;
            end
            if abs(sin(q(6)))>thresh
                sq5=-Q(3,2)/sin(q(6));
            end
            if abs(cos(q(6)))>thresh
                sq5=Q(3,1)/cos(q(6));
            end
            q(5)=atan2(sq5,cq5);

        else %degenerate solution, in this case, q4 cannot be determined,
             % so q(4)=0 is assigned
            if wrist==1 %wrist up
                q(4)=0;
                q(5)=0;
                q(6)=atan2(-Q(1,2)+Q(2,1),Q(1,1)+Q(2,2));
            else %wrist down
                q(4)=-pi;
                q(5)=0;
                q(6)=atan2(-Q(1,2)+Q(2,1),Q(1,1)+Q(2,2))+pi;
            end

        end

       %geometric solution
    case 'geometric'
        % T is the noa matrix defining the position/orientation of the end
        % effector's reference system
        vx6=T(1:3,1);
        vz5=T(1:3,3); % The vector a z6=T(1:3,3) is coincident with z5

        % Obtain the position and orientation of the system 3
        % using the already computed joints q1, q2 and q3
        T01=dh(robot, q, 1);
        T12=dh(robot, q, 2);
        T23=dh(robot, q, 3);
        T03=T01*T12*T23;

        vx3=T03(1:3,1);
        vy3=T03(1:3,2);
        vz3=T03(1:3,3);

        % find z4 normal to the plane formed by z3 and a
        vz4=cross(vz3, vz5);	% end effector's vector a: T(1:3,3)

        % in case of degenerate solution,
        % when vz3 and vz6 are parallel--> then z4=0 0 0, choose q(4)=0 as solution
        if norm(vz4) <= 0.000001
            if wrist == 1 %wrist up
                q(4)=0;
            else
                q(4)=-pi; %wrist down
            end
        else
            %this is the normal and most frequent solution
            cosq4=wrist*dot(vy3,vz4);
            sinq4=wrist*dot(-vx3,vz4);
            q(4)=atan2(sinq4, cosq4);
        end
        %propagate the value of q(4) to compute the system 4
        T34=dh(robot, q, 4);
        T04=T03*T34;
        vx4=T04(1:3,1);
        vy4=T04(1:3,2);

        % solve for q5
        cosq5=dot(-vy4,vz5);
        sinq5=dot(vx4,vz5);
        q(5)=atan2(sinq5, cosq5);

        %propagate now q(5) to compute T05
        T45=dh(robot, q, 5);
        T05=T04*T45;
        vx5=T05(1:3,1);
        vy5=T05(1:3,2);

        % solve for q6
        cosq6=dot(vx6,vx5);
        sinq6=dot(vx6,vy5);
        q(6)=atan2(sinq6, cosq6);

    otherwise
        disp('no method specified in solve_spherical_wrist');
end


% For transforming adjacency matrices to random orders,
% need dataset saved as "name_random.mat".

dataset = strvcat('MUTAG_random');
for ith_data = 1: size(dataset, 1)
    clear save_struct
    data = strcat(dataset(ith_data, :))
    dataname = strcat('../data/raw_data/', data, '.mat');
    load(dataname);
    X = eval(data(1:end-7));
    for i = 1: size(X, 2)
        am = full(X(i).am);
        l = X(i).nl.values;
        %[~, ~, am, l] = palette_wl(am, l); % transform to wl vertex order
        perm = randperm(length(l));
        X(i).am = am(perm, perm);
        X(i).nl.values = l(perm);
    end
    save_struct.(data(1:end-7)) = X;
    save(dataname, '-struct', 'save_struct', '-append');
end


function scrollplot(s)

    % Created by Steven Lord, slord@mathworks.com
    % Uploaded to MATLAB Central
    % http://www.mathworks.com/matlabcentral
    % 7 May 2002
    %
    % Permission is granted to adapt this code for your own use.
    % However, if it is reposted this message must be intact.

    % Generate and plot data
    x=s.dnum();
    y=s.data();
    dx=1;
    %% dx is the width of the axis 'window'
    a=gca;
    p=plot(x,y);

    % Set appropriate axis limits and settings
    set(gcf,'doublebuffer','on');
    %% This avoids flickering when updating the axis
    set(a,'xlim',[min(x) min(x)+dx]);
    set(a,'ylim',[min(y) max(y)]);

    % Generate constants for use in uicontrol initialization
    pos=get(a,'position');
    Newpos=[pos(1) pos(2)-0.1 pos(3) 0.05];
    %% This will create a slider which is just underneath the axis
    %% but still leaves room for the axis labels above the slider
    xmax=max(x);
    xmin=min(x);
    xmin=0;
    %gs = get(gcbo,'value')+[min(x) min(x)+dx]
    S=sprintf('set(gca,''xlim'',get(gcbo,''value'')+[%f %f])',[xmin xmin+dx])
    %% Setting up callback string to modify XLim of axis (gca)
    %% based on the position of the slider (gcbo)
    % Creating Uicontrol
    h=uicontrol('style','slider',...
        'units','normalized','position',Newpos,...
        'callback',S,'min',xmin,'max',xmax-dx);
        %'callback',S,'min',0,'max',xmax-dx);
end


function [volume, flipvecout] = volumeflip(volume, flipvecin)

% VOLUMEFLIP
%
% See also VOLUMEPERMUTE, ALIGN_IJK2XYZ, ALIGN_XYZ2IJK

if nargin<2
  flipvecin = 'auto';
end

% do a low-level check on the input data
if ~isfield(volume, 'transform'), ft_error('the input volume needs a transformation matrix'); end
if ~isfield(volume, 'dim'),       ft_error('the input volume needs a dim field');             end

isrighthanded = det(volume.transform(1:3,1:3))>0;

% check the requested behavior
if ischar(flipvecin)
  switch flipvecin
    case {'left' 'lefthanded'}
      if isrighthanded
        ft_warning('left-handed axes system is requested, performing flip');
        flipvecin = [1 0 0];
      else
        flipvecin = [0 0 0];
      end
    case {'right' 'righthanded'}
      if ~isrighthanded
        ft_warning('right-handed axes system is requested, performing flip');
        flipvecin = [1 0 0];
      else
        flipvecin = [0 0 0];
      end
    case 'auto'
    otherwise
      ft_error('unsupported option specified');
  end
end

% get the largest values on the main diagonal of the transformation matrix
[volume, P] = volumepermute(volume);

% define some variable locally
T   = volume.transform;
dim = volume.dim;

% determine which fields can be flipped
fn = parameterselection('all', volume);

% pre-allocate
flipvecout = false(1,3);

% do the flipping on the numeric data and transformation matrix
for m = 1:3
  if ischar(flipvecin)
    flipvecout(m) = T(m,m)<0;
  else
    flipvecout(m) = flipvecin(m);
  end

  if flipvecout(m)
    % get the reflection matrix
    flipmat = eye(4); flipmat(m,m) = -1; flipmat(m,4) = dim(m)+1;
    for k = 1:numel(fn)
      tmp = volume.(fn{k});
      volume.(fn{k}) = flipdim(tmp, m);
    end
    T = T*flipmat;
  else
    % do nothing
  end
end

% update the transformation matrix
volume.transform = T;

% permute back
volume = volumepermute(volume, P);