 function G = Gtomo2_strip(sg, ig, varargin)
%function G = Gtomo2_strip(sg, ig, options)
%
% Generate a 2D system matrix for tomographic image reconstruction,
% based on a square pixel basis and strip-integral detector model,
% i.e., a rectangular detector response.
% Works for parallel-beam, arc-fan, and flat-fan geometries.
%
% Closely matches Aspire "system 13" and "system 14".
% The main limitation here is that it uses Matlab "sparse" data type,
% so you will run out of memory for large image or sinogram sizes.
%
% in:
%	sg		sino_geom()
%	ig		image_geom()
%
% options:
%	'strip_width'	if 0, then line integrals (default: sg.d)
%	'single'	if 1, then double(single()) the values, cf aspire
%	'gam_max'	max acceptance angle of detector [degrees] (def: 90)
%
% out:
%	G [nb*na,np]	Gsparse object, where np = sum(ig.mask(:))
%
% Copyright 2005-8-16, Jeff Fessler, The University of Michigan

if nargin == 1 && streq(sg, 'test'), Gtomo2_strip_test, return, end
if nargin < 2, help(mfilename), error(mfilename), end

% defaults
arg.gam_max = 90; % degrees
arg.strip_width = [];
arg.single = false;
arg.chat = false;
arg.sse = false;
arg = vararg_pair(arg, varargin);

if isempty(arg.strip_width), arg.strip_width = sg.d; end

if streq(sg.type, 'par')
  if (arg.sse)

    nthread = 1;
    mask = uint8(ig.mask);

    [A B] = jmh_sse_mex('jmh,sse,matrix',int32(nthread), ...
                        single(sg.orbit_start*pi/180), single(sg.orbit*pi/180), ...
                        int32(sg.nb), single(sg.dr), single(sg.offset_r), ...
                        single(arg.strip_width), int32(ig.nx), single(ig.dx), ...
                        single(ig.offset_x), int32(ig.ny), single(ig.dy), ...
                        single(ig.offset_y), mask,int32(sg.na));

    M = ceil((ig.dx * sqrt(2) + arg.strip_width) / sg.dr);

%     jj = [];

%     % Bad idea
%     for y=1:ig.ny
%       for x=1:ig.nx
%         for a=1:sg.na
%           for m=1:M
%             jj = [jj,(y-1)*ig.nx + x];
%           end
%         end
%       end
%     end
    jj = reshape(repmat(1:ig.nx*ig.ny,sg.na*M,1),[1,ig.nx*ig.ny*sg.na*M]);

    G = sparse(double(B+1),double(jj'),double(A),sg.na*sg.nb,ig.nx*ig.ny,length(A));

  else
      G = Gtomo2_strip_par(sg.nb, sg.na, sg.dr, sg.offset_r, ...
                           sg.orbit, sg.orbit_start, arg.strip_width, ...
                           ig.nx, ig.ny, ig.dx, ig.dy, ig.offset_x, ig.offset_y, ...
                           ig.mask, arg.single);
  end
elseif streq(sg.type, 'fan')
	G = Gtomo2_strip_fan(sg.nb, sg.na, sg.ds, sg.offset_s, ...
		sg.ar, arg.strip_width, ...
		deg2rad(arg.gam_max), ...
		sg.dsd, sg.dso, sg.dfs, ...
		sg.source_offset, ...
		ig.nx, ig.ny, ig.dx, ig.dy, ig.offset_x, ig.offset_y, ...
		ig.mask, arg.single, arg.chat);

else
	error('unknown geometry: %s', sg.type)
end

G = Gsparse(G, 'mask', ig.mask, 'idim', [ig.nx ig.ny], 'odim', [sg.nb sg.na]);

%
% = Gtomo2_strip_par()
%
function G = Gtomo2_strip_par(nb, na, dr, offset_r, ...
	orbit, orbit_start, strip_width, ...
	nx, ny, dx, dy, offset_x, offset_y, mask, is_single);

% pixel centers
wx = (nx-1)/2 + offset_x;
wy = (ny-1)/2 + offset_y;
wb = (nb-1)/2 + offset_r;

x = dx * ([0:nx-1] - wx);
y = dy * ([0:ny-1] - wy); % caution, may not match aspire if offset_y != 0
[x y] = ndgrid(x, y);
x = x(mask(:));
y = y(mask(:));
np = length(x);		% sum(mask(:)) - total # of support pixels

angle = deg2rad(orbit_start + [0:na-1]'/na * orbit);
cang = cos(angle);
sang = sin(angle);
tau = cang * x' + sang * y'; % [na,np] projected pixel center
tau = tau / dr;
tau = tau + wb;

d_max = (abs(cang) + abs(sang)) / 2;
tau_max = (dx * d_max + strip_width/2) / dr;
ib_min = 1 + floor(tau - repmat(tau_max, 1, np));
M = ceil((dx * sqrt(2) + strip_width) / dr);
jj = find(mask(:))'; % all-column G
jj = repmat(jj, na, 1);
jj = col(jj); % so that na=1 case works
list.ii = [];
list.jj = [];
list.ss = [];
for mm=0:M-1
	ticker(mfilename, mm+1, M)
	ib = ib_min + mm;
	good = (ib >= 0) & (ib < nb);
	good = good(:);
	val = square_strip_int(dr * (ib - tau), ...
		repmat(angle, 1, np), 'dx', dx, 'sw', strip_width);
	val = val(:);
	ii = col(ib + repmat([0:na-1]'*nb, 1, np)); % sinogram index
	list.ii = [list.ii; 1+ii(good)];
	list.jj = [list.jj; jj(good)];
	list.ss = [list.ss; val(good)];
end
if is_single
	list.ss = double(single(list.ss)); % stupid matlab insists on double
end
G = sparse(list.ii, list.jj, list.ss, nb*na, nx*ny, length(list.ss));

%
% = Gtomo2_strip_fan()
%
function G = Gtomo2_strip_fan(nb, na, ds, offset_s, ...
	beta, strip_width, ...
	gam_max, ... % maximum angle gamma accepted by detector [radians]
	dsd, dso, dfs, roff, ...
	nx, ny, dx, dy, offset_x, offset_y, mask, is_single, chat);

% pixel centers
wx = (nx-1)/2 + offset_x;
wy = (ny-1)/2 + offset_y;
wb = (nb-1)/2 + offset_s;

x = dx * ([0:nx-1] - wx);
y = dy * ([0:ny-1] - wy); % caution, may not match aspire if offset_y != 0
[x y] = ndgrid(x, y);
x = x(mask(:)); % [np,1]
y = y(mask(:));
np = length(x); % sum(mask(:)) - total # of support pixels
na = length(beta);

cbet = cos(beta);
sbet = sin(beta);

% s0 is "s" value for center of each pixel
gam0 = atan2(cbet * x' + sbet * y' - roff, ...
	dso + sbet * x' - cbet * y'); % [na,np]
clear cbet sbet
if dfs == 0 % 3rd gen (arc)
	s0 = dsd * gam0;
elseif isinf(dfs)
	s0 = dsd * tan(gam0);
else
	error 'unsupported dfs'
end

ang0 = repmat(beta, [1 np]) + gam0;
mag0 = dso * cos(gam0) - roff * sin(gam0) ...
	+ repmat(x', [na 1]) .* sin(ang0) - repmat(y', [na 1]) .* cos(ang0);
gamgood = abs(gam0) < gam_max;
if chat, printm('gam0: %g %g', rad2deg(minmax(gam0(gamgood)))), end
clear gam0
if dfs == 0 % 3rd gen (arc)
	mag0 = mag0 / dsd;
elseif isinf(dfs)
	mag0 = mag0 / dsd ./ (1 + (s0 ./ dsd).^2);
else
	error 'dfs bug'
end
mag0min = min(mag0(gamgood));
if isempty(mag0min)
	G = sparse([], [], [], nb*na, nx*ny, 0);
	return
end

tau = s0 / ds + wb; % [na,np], unitless

M = ceil((dx/mag0min * sqrt(2) + strip_width) / ds); % conservative!
if chat, printm('M=%d, mag0min=%g', M, mag0min), end
if M > 100
	printm('Warn: M=%d too large? probably x-ray source is too oblique', M)
	printm('type "return" and hope for the best, but it may be *slow*')
	printm('recommend using "gam_max" to impose incidence angle constraint')
	keyboard
end

ib_min = 1 + floor(tau - M/2);
jj = find(mask(:))';	% all-column G
jj = repmat(jj, na, 1); % [na,np]
jj = col(jj); % so that na=1 case works
list.ii = [];
list.jj = [];
list.ss = [];
for mm=0:M-1
	ticker(mfilename, mm+1, M)
	ib = ib_min + mm;
	val = mag0 .* square_strip_int(ds * (ib - tau), ...
		ang0, 'dx', dx ./ mag0, ...
		'sw', strip_width);
	val = val(:); % for na=1 case
%	val = square_strip_int(ds * (ib - tau) .* mag0, ...
%		ang0, 'dx', dx, 'sw', strip_width .* mag0); % same!
	ii = col(ib + repmat([0:na-1]'*nb, 1, np)); % sinogram index
%	if chat, printm('%d of %d zeros', sum(val(:)==0), length(val(:))), end
	good = (ib >= 0) & (ib < nb);
	good = good & gamgood;
	good = good(:);
	good = good & (val > 0); % remove extra zeros due to conservative M
	list.ii = [list.ii; 1+ii(good)];
	list.jj = [list.jj; jj(good)];
	list.ss = [list.ss; val(good)];
end
if is_single
	list.ss = double(single(list.ss)); % stupid matlab insists on double
end
G = sparse(list.ii, list.jj, list.ss, nb*na, nx*ny, length(list.ss));

%
% test demo
%
function Gtomo2_strip_test
ig = image_geom('nx', 512, 'ny', 480', 'fov', 500, 'down', 8);
ig.mask = ig.circ > 0;
% todo: 3 cases to test: parallel, arc-fan, flat-fan
sg = sino_geom('par', 'nb', 600, 'na', 480, 'dr', 1.2, 'down', ig.down);
% sg = sino_geom('fan', 'nb', 888, 'na', 984, 'ds', 1.0, 'down', ig.down, ...
% 	'offset_s', 0*0.25, 'source_offset', 0*3.0, ...
% 	'dsd', 949, 'dod', 408, 'dfs', 0);

ell = [20 50 150 150 0 1];
ell = [];
[x ell] = ellipse_im(ig, ell, 'oversample', 2);
ya = ellipse_sino(sg, ell, 'oversample', 4);

G = Gtomo2_strip(sg, ig, 'chat', 0);

yd = G * x;
sino = sg.zeros; sino(sg.nb/2, 10) = 1;
b1 = G' * sino;
bu = G' * sg.ones;
max_percent_diff(min(bu(ig.mask)), max(bu(ig.mask)), 'backproject ones')

if im
	clf, im pl 2 2
	im(1, x, 'test image')
	im(2, ya, 'sinogram ya'), cbar
	im(4, yd, 'sinogram yd'), cbar
	im(3, yd-ya, 'yd-ya'), cbar
	if 0
		im(1, ig.mask, 'support mask')
		im(2, b1, 'backproject 1 ray'), cbar
		im(3, bu, 'backproject ones'), cbar
	end
end

% verify consistency with Gtomo2_wtmex (aspire)
if 1 & has_aspire %& streq(sg.type, 'par')
prompt
	Gw = Gtomo2_wtmex(sg, ig, 'pairs', {'strip_width', sg.d});
	yw = Gw * x;
	ys = G * x;
	max_percent_diff(yw, ys, 'sino Gtomo2_wtmex vs Gtomo2_strip')
	if im
		im pl 2 3, im(1, x), cbar
		im(1, ya, 'ya'), cbar
		im(2, ys, 'ys'), cbar
		im(3, yw, 'yw'), cbar
		im(4, ys-yw, 'ys-yw'), cbar
		xlabelf('%g%%', 100*nrms(ys(:), yw(:)))
		im(5, ys-ya, 'ys-ya'), cbar
		xlabelf('%g%%', 100*nrms(ys(:), ya(:)))
		im(6, yw-ya, 'yw-ya'), cbar
		xlabelf('%g%%', 100*nrms(yw(:), ya(:)))
	end
end


function [h, compUpAP, compUp] =  lfmComputeH3AP(gamma1_p, gamma1_m, sigma2, t1, ...
    t2, preFactor, mode)

% LFMCOMPUTEH3AP Helper function for computing part of the LFMAP kernel.
% FORMAT
% DESC computes a portion of the LFMAP kernel.
% ARG gamma1 : Gamma value for first system.
% ARG gamma2 : Gamma value for second system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG preFactor : precomputed constants.
% ARG mode: indicates the correct derivative.
% RETURN h : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : Mauricio Alvarez, 2010

% KERN

% Evaluation of h

if nargout>1
    [compUpAP{1}, compUp{1}] = lfmapComputeUpsilonMatrix(gamma1_p,sigma2, t1,t2, mode);
    [compUpAP{2}, compUp{2}] = lfmapComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);
    h = preFactor(1)*compUpAP{1} + preFactor(2)*compUpAP{2};
else
    h = preFactor(1)*lfmapComputeUpsilonMatrix(gamma1_p,sigma2, t1,t2, mode) ...
        + preFactor(2)*lfmapComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);
end


%% Misorientation Distribution Function
%
%% TODO: Please help to extend this section
% Let us consider the uncorrelated missorientation ODF corresponding to our
% model ODF.

mtexdata titanium

odf = calcDensity(ebsd.orientations)

%%

% the uncorrelated
mdf = calcMDF(odf)

%%

plotSection(mdf,'axisAngle')

%% Axis / Angle Distribution
% Then we can plot the distribution of the rotation axes of this
% missorientation ODF

plotAxisDistribution(mdf)

%%
% and the distribution of the missorientation angles and compare them to a
% uniform ODF

close all
plotAngleDistribution(mdf)
hold all
plotAngleDistribution(ebsd.CS,ebsd.CS)
hold off
legend('model ODF','uniform ODF')


function PlotAggregationVariance(AggregationVariance,SimulSecMoms,SecMom_Empirical,...
    SecMom_Annualized,NumSimulations,b,s_2,Name)
figure
for n=1:NumSimulations
    hold on
    h=plot(AggregationVariance,SimulSecMoms(:,n),'.');
    set(h,'color','k','markersize',1)
end
hold on
h=plot(AggregationVariance,SecMom_Empirical);
set(h,'color','r','linewidth',2);
hold on
h=plot(AggregationVariance,SecMom_Annualized);
set(h,'color','k','linewidth',2);
set(gca,'ytick',[],'xlim',[AggregationVariance(1) AggregationVariance(end)])
grid on
xlabel('aggregation size (days)')
ylabel('aggregated variance')
set(gcf,'Name',['               long memory of ' Name ' par swap rate'])

figure
h2=plot(log(AggregationVariance),log(SecMom_Empirical),'.');
%set(h,'color','k');
hold on
%h2=plot(log(AggregationVariance),b(1)+b(2)*log(AggregationVariance),'r');
hold on
y_norm=log(s_2)+log(AggregationVariance);
h3=plot(log(AggregationVariance),y_norm,'k');
grid on
legend([h2 h3],['frac. B. m.: H=' num2str(b(2)/2)],'B. m.: H=0.5','location','northwest')
xlabel('log-aggregation size (days)')
ylabel('log-aggregated variance')
set(gcf,'Name',['               long memory of ' Name ' par swap rate'])


function lambda = orth_symm_eigenvalues ( n )

%*****************************************************************************80
%
%% ORTH_SYMM_EIGENVALUES returns eigenvalues of the ORTH_SYMM matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  m = floor ( ( n + 1 ) / 2 );
  lambda(1:m,1)   = +1.0;
  lambda(m+1:n,1) = -1.0;

  return
end


function i4row_sum_test ( )

%*****************************************************************************80
%
%% I4ROW_SUM_TEST tests I4ROW_SUM;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  m = 3;
  n = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4ROW_SUM_TEST\n' );
  fprintf ( 1, '  I4ROW_SUM computes row sums;\n' );

  k = 0;
  for i = 1 : m
    for j = 1 : n
      k = k + 1;
      a(i,j) = k;
    end
  end

  i4mat_print ( m, n, a, '  The matrix:' );

  rowsum(1:m) = i4row_sum ( m, n, a );

  mean(1:m) = i4row_mean ( m, n, a );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Sum, mean:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : m
    fprintf ( 1, '  %6d  %6d  %9f\n', i, rowsum(i), mean(i) );
  end

  return
end


function F=makeRFSfilters
% Returns the RFS filter bank of size 49x49x38 in F. The MR8, MR4 and
% MRS4 sets are all derived from this filter bank. To convolve an
% image I with the filter bank you can either use the matlab function
% conv2, i.e. responses(:,:,i)=conv2(I,F(:,:,i),'valid'), or use the
% Fourier transform.

  SUP=49;                 % Support of the largest filter (must be odd)
  SCALEX=[1,2,4];         % Sigma_{x} for the oriented filters
  NORIENT=6;              % Number of orientations

  NROTINV=2;
  NBAR=length(SCALEX)*NORIENT;
  NEDGE=length(SCALEX)*NORIENT;
  NF=NBAR+NEDGE+NROTINV;
  F=zeros(SUP,SUP,NF);
  hsup=(SUP-1)/2;
  [x,y]=meshgrid([-hsup:hsup],[hsup:-1:-hsup]);
  orgpts=[x(:) y(:)]';

  count=1;
  for scale=1:length(SCALEX),
    for orient=0:NORIENT-1,
      angle=pi*orient/NORIENT;  % Not 2pi as filters have symmetry
      c=cos(angle);s=sin(angle);
      rotpts=[c -s;s c]*orgpts;
      F(:,:,count)=makefilter(SCALEX(scale),0,1,rotpts,SUP);
      F(:,:,count+NEDGE)=makefilter(SCALEX(scale),0,2,rotpts,SUP);
      count=count+1;
    end;
  end;
  F(:,:,NBAR+NEDGE+1)=normalise(fspecial('gaussian',SUP,10));
  F(:,:,NBAR+NEDGE+2)=normalise(fspecial('log',SUP,10));
return

function f=makefilter(scale,phasex,phasey,pts,sup)
  gx=gauss1d(3*scale,0,pts(1,:),phasex);
  gy=gauss1d(scale,0,pts(2,:),phasey);
  f=normalise(reshape(gx.*gy,sup,sup));
return

function g=gauss1d(sigma,mean,x,ord)
% Function to compute gaussian derivatives of order 0 <= ord < 3
% evaluated at x.

  x=x-mean;num=x.*x;
  variance=sigma^2;
  denom=2*variance;
  g=exp(-num/denom)/sqrt(pi*denom);
  switch ord,
    case 1, g=-g.*(x/variance);
    case 2, g=g.*((num-variance)/(variance^2));
  end;
return

function f=normalise(f), f=f-mean(f(:)); f=f/sum(abs(f(:))); return


function Y = vl_nnsoftmaxloss(X,c,dzdy)
% VL_NNSOFTMAXLOSS  CNN combined softmax and logistic loss
%    Y = VL_NNSOFTMAX(X, C) applies the softmax operator followed by
%    the logistic loss the data X. X has dimension H x W x D x N,
%    packing N arrays of W x H D-dimensional vectors.
%
%    C contains the class labels, which should be integer in the range
%    1 to D.  C can be an array with either N elements or with H x W x
%    1 x N dimensions. In the fist case, a given class label is
%    applied at all spatial locations; in the second case, different
%    class labels can be specified for different locations.
%
%    D can be thought of as the number of possible classes and the
%    function computes the softmax along the D dimension. Often W=H=1,
%    but this is not a requirement, as the operator is applied
%    convolutionally at all spatial locations.
%
%    DZDX = VL_NNSOFTMAXLOSS(X, C, DZDY) computes the derivative DZDX
%    of the CNN with respect to the input X given the derivative DZDY
%    with respect to the block output Y. DZDX has the same dimension
%    as X.

% Copyright (C) 2014 Andrea Vedaldi.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

%X = X + 1e-6 ;
sz = [size(X,1) size(X,2) size(X,3) size(X,4)] ;

% index from 0
c = c - 1 ;

if numel(c) == sz(4)
  % one label per image
  c = reshape(c, [1 1 1 sz(4)]) ;
  c = repmat(c, [sz(1) sz(2)]) ;
else
  % one label per spatial location
  sz_ = [size(c,1) size(c,2) size(c,3) size(c,4)] ;
  assert(isequal(sz_, [sz(1) sz(2) 1 sz(4)])) ;
end

% convert to indeces
c_ = 0:numel(c)-1 ;
c_ = 1 + ...
  mod(c_, sz(1)*sz(2)) + ...
  (sz(1)*sz(2)) * c(:)' + ...
  (sz(1)*sz(2)*sz(3)) * floor(c_/(sz(1)*sz(2))) ;

% compute softmaxloss
Xmax = max(X,[],3) ;
ex = exp(bsxfun(@minus, X, Xmax)) ;

n = sz(1)*sz(2) ;
if nargin <= 2
  t = Xmax + log(sum(ex,3)) - reshape(X(c_), [sz(1:2) 1 sz(4)]) ;
  Y = sum(t(:)) / n ;
else
  Y = bsxfun(@rdivide, ex, sum(ex,3)) ;
  Y(c_) = Y(c_) - 1;
  Y = Y * (dzdy / n) ;
end


function [X,Y,vals,labI]=mp_conic(optn,varargin)
% MP_CONIC  Conic projections
%           This function should not be used directly; instead it is
%           is accessed by various high-level functions named M_*.

% Rich Pawlowicz (rich@ocgy.ubc.ca) 2/Apr/1997
%
% This software is provided "as is" without warranty of any kind. But
% it's mine, so you can't sell it.
%
% Mathematical formulas for the projections and their inverses are taken from
%
%      Snyder, John P., Map Projections used by the US Geological Survey,
%      Geol. Surv. Bull. 1532, 2nd Edition, USGPO, Washington D.C., 1983.
%
% These are  conic projections with two standard parallels, useful
% for showing limited areas at mid-latitudes.
%    Albers equal-area - has an equal-area property
%    Lambert conformal - is conformal
%
%  7/6/99 - fixed tendency to re-define .ulongs if .clong set by user
%  3/4/02 - added error if parallels are equidistant from equator (i.e. not conic projection really)
%  24/10/08 - added ellipsoidal earth computations for lambert conformal conic projection
%  16/10/09 - added ellipsoidal earth computations for albers conic projection
%  06/03/17 - changed 'false_origin' to 'origin' as option name, also it
%             didn't work correctly with the 'normal' spheroid so this was
%             fixed, AND changed the default parallels so they were at 25%
%             and 75% limits instead of being a single parallel at the
%             center to prevent blowups with the albers ellipsoidal
%             projection AND made it work with SPHERE ellipsoid.

global MAP_PROJECTION MAP_VAR_LIST

MAP_ELLIP=mc_ellips;

name={'Albers Equal-Area Conic','Lambert Conformal Conic'};

pi180=pi/180;

switch optn

  case 'name'

     X=name;

  case {'usage','set'}

     m_names=fieldnames(MAP_ELLIP);

     X=char({['     ''' varargin{1} ''''],...
              '     <,''lon<gitude>'',[min max]>',...
              '     <,''lat<itude>'',[min max]>',...
              '     <,''clo<ngitude>'',value>',...
              '     <,''par<allels>'',[lat1 lat2]>',...
              '     <,''rec<tbox>'', ( ''on'' | ''off'' )>',...
              '     <,''ell<ipsoid>'', one of',...
    reshape(sprintf('         %6s',m_names{:}),15,length(m_names))',...
        '                               >',...
	      '     <,''ori<gin>'', [long lat]>'});

  case 'get'

     X=char([' Projection: ' MAP_PROJECTION.name '  (function: ' MAP_PROJECTION.routine ')'],...
            [' longitudes: ' num2str(MAP_VAR_LIST.ulongs) ' (centered at ' num2str(MAP_VAR_LIST.clong) ')'],...
            [' latitudes: ' num2str(MAP_VAR_LIST.ulats) ],...
            [' standard parallels: ' num2str(MAP_VAR_LIST.parallels) ],...
            [' Rectangular border: ' MAP_VAR_LIST.rectbox ],...
            [' ellipsoid: ' MAP_VAR_LIST.ellipsoid ],...
            [' origin: ' num2str(MAP_VAR_LIST.origin) ]);

  case 'initialize'

    MAP_VAR_LIST=[];
    MAP_PROJECTION.name=varargin{1};
    MAP_VAR_LIST.ulongs=[-180 -50];
    MAP_VAR_LIST.ulats=[10 85];
    MAP_VAR_LIST.parallels=NaN;
    MAP_VAR_LIST.clong=NaN;
    MAP_VAR_LIST.origin=NaN;
    MAP_VAR_LIST.rectbox='off';
    MAP_VAR_LIST.ellipsoid = 'normal';
    MAP_VAR_LIST.aussiemode=false;
    k=2;longs_def=0;
    while k<length(varargin)
      switch varargin{k}(1:3)
         case 'lon'
           MAP_VAR_LIST.ulongs=varargin{k+1}(:)';longs_def=1;
           if MAP_VAR_LIST.ulongs(1)>MAP_VAR_LIST.ulongs(2)
              MAP_VAR_LIST.ulongs=MAP_VAR_LIST.ulongs([2 1]);
           end
         case 'clo'
           MAP_VAR_LIST.clong=varargin{k+1};
         case 'lat'
           MAP_VAR_LIST.ulats=varargin{k+1}(:)';
         case 'par'
           MAP_VAR_LIST.parallels=varargin{k+1};
         case 'rec'
             switch lower(varargin{k+1}(1:2))
                 case {'on','bo'}
                    MAP_VAR_LIST.rectbox='on';
                 case 'of'
                    MAP_VAR_LIST.rectbox='on';
                 otherwise
                     error(['m_proj: Unrecognized box option: ' varargin{k+1}]);
             end
         case 'ell'
           MAP_VAR_LIST.ellipsoid=varargin{k+1};
         case 'ori'
           MAP_VAR_LIST.origin=varargin{k+1};
         case 'fal'
           error(' FALSE_ORIGIN option has been renamed to ORIGIN - Change your m_proj call! ');
	 case 'aus'  % aussiemode - my joke
	   if strcmp(varargin{k+1},'on')
	     MAP_VAR_LIST.aussiemode=true;
	   end
         otherwise
           disp(['Unknown option: ' varargin{k}]);
      end
       k=k+2;
    end
    if isnan(MAP_VAR_LIST.clong)
      if isnan(MAP_VAR_LIST.origin)
         MAP_VAR_LIST.clong=mean(MAP_VAR_LIST.ulongs);
      else
         MAP_VAR_LIST.clong=MAP_VAR_LIST.origin(1);
      end
    elseif  ~longs_def
        MAP_VAR_LIST.ulongs=MAP_VAR_LIST.clong+[-180 180];
    end
    if isnan(MAP_VAR_LIST.parallels), MAP_VAR_LIST.parallels=mean(MAP_VAR_LIST.ulats)*[1 1]+diff(MAP_VAR_LIST.ulats)*[-1/6 1/6]; end % change default mar/2017
    if isnan(MAP_VAR_LIST.origin), MAP_VAR_LIST.origin=[MAP_VAR_LIST.clong mean(MAP_VAR_LIST.parallels)]; end

    MAP_VAR_LIST.rlongs=MAP_VAR_LIST.ulongs*pi180;
    MAP_VAR_LIST.rlats=MAP_VAR_LIST.ulats*pi180;
    MAP_VAR_LIST.rparallels=MAP_VAR_LIST.parallels*pi180;
    MAP_VAR_LIST.rorigin=MAP_VAR_LIST.origin*pi180;

    MAP_VAR_LIST.ellip=getfield(MAP_ELLIP,MAP_VAR_LIST.ellipsoid);

    % These are constants used by the projection formulas

    switch MAP_PROJECTION.name
      case name(1)
        if MAP_VAR_LIST.ellip(2)==0 % spherical
           MAP_VAR_LIST.n=sum(sin(MAP_VAR_LIST.rparallels))/2;
           if MAP_VAR_LIST.n==0
               error('Your parallels are equidistant from the equator - use a cylindrical projection!');
           end
           MAP_VAR_LIST.C=cos(MAP_VAR_LIST.rparallels(1)).^2+2*MAP_VAR_LIST.n*sin(MAP_VAR_LIST.rparallels(1));
           MAP_VAR_LIST.rho0=MAP_VAR_LIST.ellip(1)*sqrt(MAP_VAR_LIST.C-2*MAP_VAR_LIST.n*sin( MAP_VAR_LIST.rorigin(2)   ))/MAP_VAR_LIST.n;
        else
 	       e=sqrt(2*MAP_VAR_LIST.ellip(2)-MAP_VAR_LIST.ellip(2)^2);
	       m12=cos(MAP_VAR_LIST.rparallels)./sqrt(1-(e.*sin(MAP_VAR_LIST.rparallels)).^2);
	       q12=(1-e.^2)*(sin(MAP_VAR_LIST.rparallels)./(1-(e.*sin(MAP_VAR_LIST.rparallels)).^2) - ...
	            1./(2*e)*log((1-e.*sin(MAP_VAR_LIST.rparallels))./(1+e.*sin(MAP_VAR_LIST.rparallels))) );
	       q0= (1-e.^2)*(sin(MAP_VAR_LIST.rorigin(2))./(1-(e.*sin(MAP_VAR_LIST.rorigin(2))).^2) - ...
	            1./(2*e)*log((1-e.*sin(MAP_VAR_LIST.rorigin(2)))./(1+e.*sin(MAP_VAR_LIST.rorigin(2)))) );
           if diff( MAP_VAR_LIST.rparallels )==0
	          MAP_VAR_LIST.n=sin(MAP_VAR_LIST.rparallels(1));
           else
	          MAP_VAR_LIST.n=-diff(m12.^2)/diff(q12);
           end
	       MAP_VAR_LIST.C=m12(1).^2 + MAP_VAR_LIST.n.*q12(1);
	       MAP_VAR_LIST.rho0=MAP_VAR_LIST.ellip(1)*sqrt(MAP_VAR_LIST.C-MAP_VAR_LIST.n*q0)/MAP_VAR_LIST.n;
        end
      case name(2)
        if strcmp(MAP_VAR_LIST.ellipsoid,'normal')
           if diff(MAP_VAR_LIST.parallels)==0
             MAP_VAR_LIST.n=sin(MAP_VAR_LIST.rparallels(1));
           else
             MAP_VAR_LIST.n=-diff(log(cos(MAP_VAR_LIST.rparallels)))/diff(log(tan(MAP_VAR_LIST.rparallels/2+pi/4)));
           end
           MAP_VAR_LIST.F=cos(MAP_VAR_LIST.rparallels(1))/MAP_VAR_LIST.n* ...
                	  tan(pi/4+MAP_VAR_LIST.rparallels(1)/2).^MAP_VAR_LIST.n;
           MAP_VAR_LIST.rho0=MAP_VAR_LIST.F/tan(pi/4+ MAP_VAR_LIST.rorigin(2)/2).^MAP_VAR_LIST.n;
        else
 	      e=sqrt(2*MAP_VAR_LIST.ellip(2)-MAP_VAR_LIST.ellip(2)^2);
	      m12=cos(MAP_VAR_LIST.rparallels)./sqrt(1-(e.*sin(MAP_VAR_LIST.rparallels)).^2);
	      t12=tan(pi/4-MAP_VAR_LIST.rparallels/2)./( (1-e*sin(MAP_VAR_LIST.rparallels))./(1+e*sin(MAP_VAR_LIST.rparallels)) ).^(e/2);
	      tF=tan(pi/4-MAP_VAR_LIST.rorigin(2)/2)./( (1-e*sin(MAP_VAR_LIST.rorigin(2)))./(1+e*sin(MAP_VAR_LIST.rorigin(2))) ).^(e/2);
	      if diff(MAP_VAR_LIST.rparallels)==0
	         MAP_VAR_LIST.n=sin(MAP_VAR_LIST.rparallels(1));
          else
             MAP_VAR_LIST.n=diff(log(m12))/diff(log(t12));
          end
	      MAP_VAR_LIST.F=m12(1)/MAP_VAR_LIST.n/t12(1).^MAP_VAR_LIST.n;
	      MAP_VAR_LIST.rho0=MAP_VAR_LIST.ellip(1)*MAP_VAR_LIST.F*tF.^MAP_VAR_LIST.n;
        end
    end

    % check for a valid ellipsoid. if not, use the normalized sphere

    if ~isfield(MAP_ELLIP,MAP_VAR_LIST.ellipsoid)
       MAP_VAR_LIST.ellipsoid = 'normal';
    end

    % Get X/Y and (if we are in a box) update the lat/long limits.

    mu_util('xylimits');
    if strcmp(MAP_VAR_LIST.rectbox,'on'),  mu_util('lllimits'); end

  case 'll2xy'

    long=varargin{1};
    lat=varargin{2};
    vals=zeros(size(long));

    % Clip out-of-range values (lat/long box)

    if ~strcmp(MAP_VAR_LIST.rectbox,'on') && ~strcmp(varargin{4},'off')
        vals=vals | long<=MAP_VAR_LIST.longs(1)+eps*10 | long>=MAP_VAR_LIST.longs(2)-eps*10 | ...
	              lat<=MAP_VAR_LIST.lats(1)+eps*10 |   lat>=MAP_VAR_LIST.lats(2)-eps*10;
        [long,lat]=mu_util('clip',varargin{4},long,MAP_VAR_LIST.longs(1),long<MAP_VAR_LIST.longs(1),lat);
        [long,lat]=mu_util('clip',varargin{4},long,MAP_VAR_LIST.longs(2),long>MAP_VAR_LIST.longs(2),lat);
        [lat,long]=mu_util('clip',varargin{4},lat,MAP_VAR_LIST.lats(1),lat<MAP_VAR_LIST.lats(1),long);
        [lat,long]=mu_util('clip',varargin{4},lat,MAP_VAR_LIST.lats(2),lat>MAP_VAR_LIST.lats(2),long);
    end

    switch MAP_PROJECTION.name
      case name(1)
        if MAP_VAR_LIST.ellip(2)==0   % spherical
           rho=MAP_VAR_LIST.ellip(1)*sqrt(MAP_VAR_LIST.C-2*MAP_VAR_LIST.n*sin(lat*pi180))/MAP_VAR_LIST.n;
        else
	       e=sqrt(2*MAP_VAR_LIST.ellip(2)-MAP_VAR_LIST.ellip(2)^2);
	       q= (1-e.^2)*(sin(lat*pi180)./(1-(e.*sin(lat*pi180)).^2) - ...
	            1./(2*e)*log((1-e.*sin(lat*pi180))./(1+e.*sin(lat*pi180))) );
	       rho=MAP_VAR_LIST.ellip(1)*sqrt(MAP_VAR_LIST.C-MAP_VAR_LIST.n*q)/MAP_VAR_LIST.n;
        end
      case name(2)
        if strcmp(MAP_VAR_LIST.ellipsoid,'normal')
           lat(lat==-90)=-89.999; % Prevents /0 problems in next line
           rho=MAP_VAR_LIST.F ./ tan(pi/4+lat*pi180/2).^MAP_VAR_LIST.n;
        else
	       e=sqrt(2*MAP_VAR_LIST.ellip(2)-MAP_VAR_LIST.ellip(2)^2);
	       t=tan(pi/4-lat*pi180/2)./( (1-e*sin(lat*pi180))./(1+e*sin(lat*pi180)) ).^(e/2);
	       rho=MAP_VAR_LIST.ellip(1)*MAP_VAR_LIST.F*t.^MAP_VAR_LIST.n;
        end
    end
    theta=MAP_VAR_LIST.n*(long-MAP_VAR_LIST.origin(1))*pi180;

    X=real(rho.*sin(theta));
    Y=real(MAP_VAR_LIST.rho0-rho.*cos(theta));

    % Clip out-of-range values (rectangular box)

    if strcmp(MAP_VAR_LIST.rectbox,'on') && ~strcmp(varargin{4},'off')
        vals= vals | X<=MAP_VAR_LIST.xlims(1)+eps*10 | X>=MAP_VAR_LIST.xlims(2)-eps*10 | ...
                     Y<=MAP_VAR_LIST.ylims(1)+eps*10 | Y>=MAP_VAR_LIST.ylims(2)-eps*10;
        [X,Y]=mu_util('clip',varargin{4},X,MAP_VAR_LIST.xlims(1),X<MAP_VAR_LIST.xlims(1),Y);
        [X,Y]=mu_util('clip',varargin{4},X,MAP_VAR_LIST.xlims(2),X>MAP_VAR_LIST.xlims(2),Y);
        [Y,X]=mu_util('clip',varargin{4},Y,MAP_VAR_LIST.ylims(1),Y<MAP_VAR_LIST.ylims(1),X);
        [Y,X]=mu_util('clip',varargin{4},Y,MAP_VAR_LIST.ylims(2),Y>MAP_VAR_LIST.ylims(2),X);
    end
    if MAP_VAR_LIST.aussiemode, Y=-Y; X=-X; end

  case 'xy2ll'

    pi180=pi/180;

    if MAP_VAR_LIST.aussiemode, varargin{2}=-varargin{2}; varargin{1}=-varargin{1}; end
    switch MAP_PROJECTION.name
      case name(1)
        rho=sqrt(varargin{1}.^2+(MAP_VAR_LIST.rho0-varargin{2}).^2);
        theta=atan(varargin{1}./(MAP_VAR_LIST.rho0-varargin{2}));
        if  MAP_VAR_LIST.ellip(2)==0
           Y=asin((MAP_VAR_LIST.C-(rho*MAP_VAR_LIST.n/MAP_VAR_LIST.ellip(1)).^2)/(2*MAP_VAR_LIST.n))/pi180;
        else
	       e=sqrt(2*MAP_VAR_LIST.ellip(2)-MAP_VAR_LIST.ellip(2)^2);
           q=(MAP_VAR_LIST.C - (rho.*MAP_VAR_LIST.n/MAP_VAR_LIST.ellip(1)).^2)./MAP_VAR_LIST.n;
	       % Y is computed iteratively
	       Y=asin(q/2);
	       for k=1:4
	          Y=Y+(1-(e.*sin(Y)).^2).^2./(2*cos(Y)).*( q./(1-e.^2) - sin(Y)./(1-(e.*sin(Y)).^2) + ...
	             1./(2*e)*log( (1-e*sin(Y))./(1+e*sin(Y)) ) );
           end
	       Y=Y/pi180;
        end
        % The pole is an arc in this projection, so for points inside that
        % arc there is no inverse. If so, the math above can return complex
        % values - instead set these to NaN
        if ~isreal(Y)
           Y(imag(Y)>1e-7)=NaN;
           Y=real(Y);
        end

      case name(2)
        rho=sign(MAP_VAR_LIST.n)*sqrt(varargin{1}.^2+(MAP_VAR_LIST.rho0-varargin{2}).^2);
        theta=atan(varargin{1}./(MAP_VAR_LIST.rho0-varargin{2}));
	    if strcmp(MAP_VAR_LIST.ellipsoid,'normal')
           Y=(2*atan((MAP_VAR_LIST.F./rho).^(1/MAP_VAR_LIST.n))-pi/2)/pi180;
        else
	       e=sqrt(2*MAP_VAR_LIST.ellip(2)-MAP_VAR_LIST.ellip(2)^2);
	       tp=(rho./MAP_VAR_LIST.ellip(1)/MAP_VAR_LIST.F).^(1./MAP_VAR_LIST.n);
	       % Y is computed iteratively
	       Y=pi/2 - 2*atan(tp);
	       for k=1:4
	         Y=pi/2 - 2*atan(tp.*((1-e*sin(Y))./(1+e*sin(Y))).^(e/2) );
           end
	       Y=Y/pi180;
        end
    end
    % Clip out-of-range values (lat/long box)
    X=MAP_VAR_LIST.origin(1)+(theta/MAP_VAR_LIST.n)/pi180;

  case 'xgrid'

    [X,Y,vals,labI]=mu_util('xgrid',MAP_VAR_LIST.longs,MAP_VAR_LIST.lats,varargin{1},3,varargin{2:3});

  case 'ygrid'

    [X,Y,vals,labI]=mu_util('ygrid',MAP_VAR_LIST.lats,MAP_VAR_LIST.longs,varargin{1},31,varargin{2:3});

  case 'box'

    [X,Y]=mu_util('box',31);

end


function s = mean(f, dim)
%MEAN   Average or mean value of a SPHEREFUN.
%   MEAN(F) takes the mean in the latitude-direction (default), i.e.,
%          MEAN(F) = 1/pi sum(F).
%
%   MEAN(F, DIM) takes the mean along the direction DIM. If DIM = 1 it is the
%   latitude-direction and if DIM = 2 then it is the longitude-direction.
%
% See also SPHEREFUN/MEAN2, SPHEREFUN/STD2.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Empty check:
if ( isempty( f ) )
    s = chebfun;
    return
end

if ( nargin == 1)
    % Default to the y-direction:
    dim = 1;
end
dom = f.domain;

s = sum( f, dim );
if ( dim == 1 )
    s = s / pi; % Mean in the latitude direction (default)
elseif ( dim == 2 )
    s = s / (2*pi); % Mean in the longitude direction
else
    error('CHEBFUN:SPHEREFUN:mean:dim', ...
        'Mean not in longitude (LAMBDA) or latitude (THETA) direction.')
end

end


%%% Extraction of the final intrinsic and extrinsic paramaters:

check_active_images;

if ~exist('solution_error')
   solution_error = zeros(6*n_ima + 15,1);
end;

fc = solution(1:2);%***
cc = solution(3:4);%***
alpha_c = solution(5);%***
kc = solution(6:10);%***

fc_error = solution_error(1:2);
cc_error = solution_error(3:4);
alpha_c_error = solution_error(5);
kc_error = solution_error(6:10);

% Calibration matrix:

KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1];
inv_KK = inv(KK);

% Extract the extrinsic paramters, and recomputer the collineations

for kk = 1:n_ima,

   if active_images(kk),

      omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%***
      Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%***

      omckk_error = solution_error(15+6*(kk-1) + 1:15+6*(kk-1) + 3);
      Tckk_error = solution_error(15+6*(kk-1) + 4:15+6*(kk-1) + 6);

   	Rckk = rodrigues(omckk);

   	Hkk = KK * [Rckk(:,1) Rckk(:,2) Tckk];

   	Hkk = Hkk / Hkk(3,3);

   else

      omckk = NaN*ones(3,1);
      Tckk = NaN*ones(3,1);
      Rckk = NaN*ones(3,3);
      Hkk = NaN*ones(3,3);
      omckk_error = NaN*ones(3,1);
      Tckk_error = NaN*ones(3,1);

   end;

   eval(['omc_' num2str(kk) ' = omckk;']);
   eval(['Rc_' num2str(kk) ' = Rckk;']);
   eval(['Tc_' num2str(kk) ' = Tckk;']);
   eval(['H_' num2str(kk) '= Hkk;']);
   eval(['omc_error_' num2str(kk) ' = omckk_error;']);
   eval(['Tc_error_' num2str(kk) ' = Tckk_error;']);

end;


function GMST=TAI2GAST(Jul1,Jul2,version,deltaT)
%%TAI2GAST Convert from international atomic time (TAI) to Greenwhich
%          apparent sidereal time (GAST), which is a measure of the
%          rotational direction of the Earth.
%
%INPUTS: Jul1,Jul2 Two parts of a pseudo-Julian date given in TAI. The
%                  units of the date are days. The full date is the sum of
%                  both terms. The date is broken into two parts to
%                  provide more bits of precision. It does not matter how
%                  the date is split.
%          version An optional integer specifying the theory to use for
%                  GMST. The theory chosen should be consistent with other
%                  values used in astronomical routines. Possible values
%                  are
%                  1982 Compute GAST ion accordance with the International
%                     Astronomical Union's (IAU's) 1982 model.
%                  2000 Compute GMST in line with IAU 2000 resolutions
%                     related to precession and nutation.
%                  2006 (The default if omitted) Compute GMST in line with
%                     IAU 2006 resolutions related to precession and
%                     nutation.
%           deltaT An optional parameter specifying the offset between TT
%                  and UT1 in seconds. If this parameter is omitted, then
%                  the value of the function getEOP will be used.
%
%OUTPUTS: GAST The Greenwhich apparent sideral time in radians.
%
%The function just calls TAI2TT and then TT2GAST.
%
%GAST is defined in Section 5.5.7 of [1].
%
%REFERENCES:
%[1] G. Petit and B. Luzum, IERS Conventions (2010), International Earth
%    Rotation and Reference Systems Service Std. 36, 2010.
%
%April 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

[Jul1,Jul2]=TAI2TT(Jul1,Jul2);

if(nargin==2)
    GMST=TT2GAST(Jul1,Jul2);
elseif(nargin==3)
    GMST=TT2GAST(Jul1,Jul2,version);
else
    GMST=TT2GAST(Jul1,Jul2,version,deltaT);
end

end

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


function M = euclideanfactory(m, n)
% Returns a manifold struct to optimize over m-by-n matrices.
%
% function M = euclideanfactory(m, n)
%
% Returns M, a structure describing the Euclidean space of m-by-n matrices
% equipped with the standard Frobenius distance and associated trace inner
% product as a manifold for Manopt.

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:
%  July 5, 2013 (NB): added egred2rgrad, ehess2rhess, mat, vec, tangent.

    if ~exist('n', 'var') || isempty(n)
        n = 1;
    end

    M.name = @() sprintf('Euclidean space R^(%dx%d)', m, n);

    M.dim = @() m*n;

    M.inner = @(x, d1, d2) d1(:).'*d2(:);

    M.norm = @(x, d) norm(d, 'fro');

    M.dist = @(x, y) norm(x-y, 'fro');

    M.typicaldist = @() sqrt(m*n);

    M.proj = @(x, d) d;

    M.egrad2rgrad = @(x, g) g;

    M.ehess2rhess = @(x, eg, eh, d) eh;

    M.tangent = M.proj;

    M.exp = @exp;
    function y = exp(x, d, t)
        if nargin == 3
            y = x + t*d;
        else
            y = x + d;
        end
    end

    M.retr = M.exp;

	M.log = @(x, y) y-x;

    M.hash = @(x) ['z' hashmd5(x(:))];

    M.rand = @() randn(m, n);

    M.randvec = @randvec;
    function u = randvec(x) %#ok<INUSD>
        u = randn(m, n);
        u = u / norm(u, 'fro');
    end

    M.lincomb = @lincomb;
    function v = lincomb(x, a1, d1, a2, d2) %#ok<INUSL>
        if nargin == 3
            v = a1*d1;
        elseif nargin == 5
            v = a1*d1 + a2*d2;
        else
            error('Bad usage of euclidean.lincomb');
        end
    end

    M.zerovec = @(x) zeros(m, n);

    M.transp = @(x1, x2, d) d;

    M.pairmean = @(x1, x2) .5*(x1+x2);

    M.vec = @(x, u_mat) u_mat(:);
    M.mat = @(x, u_vec) reshape(u_vec, [m, n]);
    M.vecmatareisometries = @() true;

end


function results = vl_test_hog(varargin)
% VL_TEST_HOG
vl_test_init ;

function s = setup()
s.im = im2single(vl_impattern('roofs1')) ;
[x,y]= meshgrid(linspace(-1,1,128)) ;
s.round = single(x.^2+y.^2);
s.imSmall = s.im(1:128,1:128,:) ;
s.imSmall = s.im ;
s.imSmallFlipped = s.imSmall(:,end:-1:1,:) ;

function test_basic_call(s)
cellSize = 8 ;
hog = vl_hog(s.im, cellSize) ;

function test_bilinear_orientations(s)
cellSize = 8 ;
vl_hog(s.im, cellSize, 'bilinearOrientations') ;

function test_variants_and_flipping(s)
variants = {'uoctti', 'dalaltriggs'} ;
numOrientationsRange = 3:9 ;
cellSize = 8 ;

for cellSize = [4 8 16]
  for i=1:numel(variants)
    for j=1:numel(numOrientationsRange)
      args = {'bilinearOrientations', ...
              'variant', variants{i}, ...
              'numOrientations', numOrientationsRange(j)} ;
      hog = vl_hog(s.imSmall, cellSize, args{:}) ;
      perm = vl_hog('permutation', args{:}) ;
      hog1 = vl_hog(s.imSmallFlipped, cellSize, args{:}) ;
      hog2 = hog(:,end:-1:1,perm) ;
      %norm(hog1(:)-hog2(:))
      vl_assert_almost_equal(hog1,hog2,1e-3) ;
    end
  end
end

function test_polar(s)
cellSize = 8 ;
im = s.round ;
for b = [0 1]
  if b
    args = {'bilinearOrientations'} ;
  else
    args = {} ;
  end
  hog1 = vl_hog(im, cellSize, args{:}) ;
  [ix,iy] = vl_grad(im) ;
  m = sqrt(ix.^2 + iy.^2) ;
  a = atan2(iy,ix) ;
  m(:,[1 end]) = 0 ;
  m([1 end],:) = 0 ;
  hog2 = vl_hog(cat(3,m,a), cellSize, 'DirectedPolarField', args{:}) ;
  vl_assert_almost_equal(hog1,hog2,norm(hog1(:))/1000) ;
end


function jed = transition_to_jed_common ( )

%*****************************************************************************80
%
%% TRANSITION_TO_JED_COMMON returns the Common calendar transition as a JED.
%
%  Discussion:
%
%    In the Common calendar, the last moment of the Julian calendar was
%      11:59 pm, 4 October 1582 Julian/CE,
%      11:59 pm, 14 October 1582 Gregorian.
%    The first minute of the Gregorian calendar ended at
%      12:01 am, 5 October 1582 Julian,
%      12:01 am, 15 October 1582 Gregorian/CE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 December 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real JED, the Julian Ephemeris Date of the date.
%
  jed = 2299160.5;

  return
end


function [best,curSel] = findWeakRuleSamples(data,labels,dist,binVals,bins,params)

numDim = size(data,2);

best.dim = 1;
best.error = 0.5;
best.dir = 1;
best.tr = 0;

% curBestErr = 0.5*ones(1,numDim);
% binNo = ones(1,numDim);
% bestDir = ones(1,numDim);

numS = params.numSample;

% KB: only sample if it is helpful
dosample = numS < numel(dist);

% always helpful to normalize
dist = dist / sum(dist);

% fot testing
% for dosample = [false,true],
%
%   niters = 100;
%
%   tic;
%   for iter = 1:niters,

if dosample,

  % KB: copied from randsample
  edges = min([0 cumsum(dist')],1); % protect against accumulated round-off
  edges(end) = 1; % get the upper edge exact
  [~,curSel] = histc(rand(numS,1),edges);

  if any(curSel==0),
    warning('Sanity check: some samples are not being chosen');
    curSel(curSel==0) = size(data,1);
  end

  curBins = bins(:,curSel);
  curLabels = labels(curSel);

else

  curBins = bins;
  curLabels = labels;
  curSel = true(size(labels));
end

% KB: precompute these
numBins = size(binVals,1)+1;
% indices with positive labels
idxpos = curLabels > 0;

% always normalize histograms by Z
if dosample,
  Z = size(curBins,2);
  Zpos = nnz(idxpos);
else
  Z = sum(dist);
  Zpos = sum(dist(idxpos));
end
Zneg = Z - Zpos;

fracpos = Zpos/Z;
fracneg = Zneg/Z;

if dosample,

  posCount = accummatrix(curBins(:,idxpos)',ones(Zpos,1),numBins)'/Z;
  negCount = accummatrix(curBins(:,~idxpos)',ones(Zneg,1),numBins)'/Z;

%   posCount = histc(curBins(:,idxpos),edges,2);
%   posCount = posCount(:,1:end-1) / Z;
%   negCount = histc(curBins(:,~idxpos),edges,2);
%   negCount = negCount(:,1:end-1) / Z;

else

  % weighted histogram, loop-free version

  posCount = accummatrix(curBins(:,idxpos)',dist(idxpos),numBins)';
  negCount = accummatrix(curBins(:,~idxpos)',dist(~idxpos),numBins)';

end

posLeft = cumsum(posCount,2);
posRight = fracpos - posLeft;
negLeft = cumsum(negCount,2);
negRight = fracneg - negLeft;

binErr = posRight+negLeft-posLeft-negRight;
negErr = binErr<0;
binErr(negErr) = -binErr(negErr);
dir = ones(numDim,numBins);
dir(negErr) = -1;
err = 0.5-binErr/2;
[curBestErr,binNo]= min(err(:,1:end-1),[],2);
curBestErr = curBestErr'; binNo = binNo';
bestDir = dir(sub2ind([numDim,numBins],1:numDim,binNo));

[minError minDim] = min(curBestErr); %#ok<UDIM>
best.error = minError;   best.dim = minDim;
best.dir = bestDir(minDim);   best.tr = binVals(binNo(minDim),minDim);

%   end
%
%   fprintf('dosample = %d, time = %f\n',dosample,toc);
%
% end

end

%{
function [predError dir] = getError(data,label,tr, dist)

dir = 1;
predLabel = 2*(data>tr)-1;
predError = sum(( (predLabel.*label) ~=1).*dist);

if(predError>0.5); predError = 1-predError; dir = -1; end
end
%}


function f = testfcn1(x)

if strcmpi(x,'init')
    f.options.PopInitRange = [0, 0; 4, 4] ;
    f.options.Vectorized = 'on' ;
    f.options.HybridFcn = {} ;
    f.options.Generations = 500 ;
    f.options.ConstrBoundary = 'penalize' ;
    f.LB = [0,0] ; f.UB = [] ;
    f.Aeq = [] ; f.beq = [] ;
    f.Aineq = [-1 0] ; f.bineq = [-0.01] ;
%     f.Aineq = [] ; f.bineq = [] ;
    f.nonlcon = [] ;
else
    a = 1 ; b = 0.01 ;
    f = (x(:,1).^b + x(:,2).^(1-b)).^a ;
end


% design and visualize arrays
clear(); close all;

wavelength = 1; % normalized
d = wavelength / 2;
design_ula = design_array_1d('ula', 12, d);
design_cp = design_array_1d('coprime', [4 5], d);
design_nested = design_array_1d('nested', [5 7], d);
design_mra = design_array_1d('mra', 12, d);

visualize_array(design_ula, 'VisualizeCoarray', true);
visualize_array(design_cp, 'VisualizeCoarray', true);
visualize_array(design_nested, 'VisualizeCoarray', true);
visualize_array(design_mra, 'VisualizeCoarray', true);


% File: RecognizeActions.m
%
% Copyright (C) Daphne Koller, Stanford Univerity, 2012

function [accuracy, predicted_labels] = RecognizeActions(datasetTrain, datasetTest, G, maxIter)

% INPUTS
% datasetTrain: dataset for training models, see PA for details
% datasetTest: dataset for testing models, see PA for details
% G: graph parameterization as explained in PA decription
% maxIter: max number of iterations to run for EM

% OUTPUTS
% accuracy: recognition accuracy, defined as (#correctly classified examples / #total examples)
% predicted_labels: N x 1 vector with the predicted labels for each of the instances in datasetTest, with N being the number of unknown test instances

% Train a model for each action
% Note that all actions share the same graph parameterization and number of max iterations
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% YOUR CODE HERE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[P1 logli1 ClassProb1 PairProb1] = EM_HMM(datasetTrain(1).actionData, datasetTrain(1).poseData, G, datasetTrain(1).InitialClassProb, datasetTrain(1).InitialPairProb, maxIter);

[P2 logli2 ClassProb2 PairProb2] = EM_HMM(datasetTrain(2).actionData, datasetTrain(2).poseData, G, datasetTrain(2).InitialClassProb, datasetTrain(2).InitialPairProb, maxIter);

[P3 logli3 ClassProb3 PairProb3] = EM_HMM(datasetTrain(3).actionData, datasetTrain(3).poseData, G, datasetTrain(3).InitialClassProb, datasetTrain(3).InitialPairProb, maxIter);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Classify each of the instances in datasetTest
% Compute and return the predicted labels and accuracy
% Accuracy is defined as (#correctly classified examples / #total examples)
% Note that all actions share the same graph parameterization

accuracy = 0;
predicted_labels = [];
P = [P1,P2,P3];
N = size(datasetTest.poseData,1);
I = size(datasetTest.labels,1);
K = 3;
loglikelihood = zeros(I,3);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% YOUR CODE HERE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for iclass = 1:3
  logEmissionProb = zeros(N,K);

  for i = 1:N
	  data = reshape(datasetTest.poseData(i,:,:),10,3);
	  for j = 1:10
		  if G(j,1) == 1
			  parent = data(G(j,2),:);
			  for k = 1:K
				  theta = P(iclass).clg(j).theta(k,:);
				  mu_y = sum(theta(1:4).*[1,parent]);
				  mu_x = sum(theta(5:8).*[1,parent]);
				  mu_a = sum(theta(9:12).*[1,parent]);
				  logEmissionProb(i,k) += lognormpdf(data(j,1),mu_y,P(iclass).clg(j).sigma_y(k));
				  logEmissionProb(i,k) += lognormpdf(data(j,2),mu_x,P(iclass).clg(j).sigma_x(k));
				  logEmissionProb(i,k) += lognormpdf(data(j,3),mu_a,P(iclass).clg(j).sigma_angle(k));
			  end
		  else
			  for k = 1:K
				  logEmissionProb(i,k) += lognormpdf(data(j,1),P(iclass).clg(j).mu_y(k),P(iclass).clg(j).sigma_y(k));
				  logEmissionProb(i,k) += lognormpdf(data(j,2),P(iclass).clg(j).mu_x(k),P(iclass).clg(j).sigma_x(k));
				  logEmissionProb(i,k) += lognormpdf(data(j,3),P(iclass).clg(j).mu_angle(k),P(iclass).clg(j).sigma_angle(k));
			  end
		  end
	  end
  end

  for i = 1:I
	  % construct all the three types of factors for each action and do inference to fill ClassProb and PairProb
	  m = length(datasetTest.actionData(i).marg_ind); % 1 to m represents S variables
	  F = repmat(struct('var',[],'card',[],'var',[]),1,2*m); % 1 is P(S1), 2 to m is P(Si/Si-1) and m+1 to 2m P(S/O)
	  F(1).var = 1; F(1).card = [K];F(1).val = log(P(iclass).c);
	  temp = log(reshape(P(iclass).transMatrix',1,9));
	  for j = 2:m
		  F(j).var = [j j-1]; F(j).card = [K K]; F(j).val = temp;
	  end
	  for j = 1:m
		  F(j+m).var = [j];F(j+m).card = [K];
		  F(j+m).val = logEmissionProb(datasetTest.actionData(i).marg_ind(j),:);
	  end
	  [M, PCalibrated] = ComputeExactMarginalsHMM(F);
	  loglikelihood(i,iclass) += logsumexp(PCalibrated.cliqueList(1).val);
  end
end
[temp, predicted_labels] = max(loglikelihood,[],2);
accuracy = sum(predicted_labels==datasetTest.labels)/I;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% Test file for @deltafun/times.m.

function pass = test_times(pref)

if (nargin < 1)
    pref = chebfunpref();
end
%%
% Get the tolerance:
dTol = pref.deltaPrefs.deltaTol;

d = deltafun(bndfun(@sin), struct('deltaMag', 1, 'deltaLoc', 0));
pass(1) = isempty(deltafun() .* deltafun());
pass(2) = isempty(deltafun() .* d) && isempty(d .* deltafun());

f = fun.constructor(@(x) exp(-x));
g = bndfun(@sin);
df1 = deltafun(f, []);
df2 = deltafun(g, struct('deltaMag', [0; 0; 0; 0; 1], 'deltaLoc', 0));
s = df1.*df2;
pass(3) = norm(s.deltaMag - [1, 4, 6, 4, 1].', inf) < dTol;

f = fun.constructor(@(x) exp(x));
g = bndfun(@sin);
df1 = deltafun(f, []);
df2 = deltafun(g, struct('deltaMag', [0; 0; 0; 1], 'deltaLoc', 0));
s = df1.*df2;
pass(4) = norm(s.deltaMag - [-1, 3, -3, 1].', inf) < dTol;

a = -4;
b = 4;

f1 = fun.constructor(@(x) exp(sin(x)), struct('domain', [a, b]));
d1 = .5*[0 1 0;
         0 1 0;
         1 0 1; ];
l1 = sort(.9*(a + (b-a)/2*rand(1,3)));

f2 = fun.constructor(@(x) exp(cos(x)), struct('domain', [a, b]));

df1 = deltafun(f1, struct('deltaMag', d1, 'deltaLoc', l1));
df2 = deltafun(f2, []);

s = df1 .* df2;
pass(5) = iszero(s.funPart - f1.*f2);
pass(6) = norm(s.deltaLoc - sort(l1), inf) == 0;

c1 = [ feval(diff(f2,2), l1(1));
       -2*feval(diff(f2,1), l1(1));
       feval(diff(f2,0), l1(1));    ];

c2 = [ feval(diff(f2,0)-diff(f2,1), l1(2));
       feval(diff(f2,0), l1(2));
       0;                       ];

c3 = [feval(diff(f2,2), l1(3));
      -2*feval(diff(f2,1), l1(3));
      feval(diff(f2,0), l1(3))];

deltas1 = .5*[c1, c2, c3];

err = s.deltaMag - deltas1;
pass(7) = norm(err(:), inf) < dTol;

d2 = .8*[1 0 1;
         0 1 0;
         0 1 0; ];
l2 =  .0010751808168034 + sort(.9*(b-a)/2*rand(1,3));
df2 = deltafun(f2, struct('deltaMag', d2, 'deltaLoc',l2));
%%
s = df1 .* df2;
pass(8) = iszero(s.funPart - f1.*f2);
pass(9) = norm(s.deltaLoc - sort(union(l1,l2)), inf) == 0;
%%
c1 = [ feval(diff(f1,0), l2(1));
       0;
       0; ];
c2 = [ feval(diff(f1, 2)-diff(f1, 1), l2(2));
       feval(diff(f1, 0)-2*diff(f1, 1), l2(2));
       feval(diff(f1, 0), l2(2));  ];

c3 = [feval(diff(f1,0), l2(3));
      0;
      0; ];

deltas2 = .8*[c1, c2, c3];

err = [deltas1, deltas2] - s.deltaMag;
pass(10) = norm(err(:), inf) < dTol;
%%
end


function [rp,nodenew,elemnew] = recoverP02P1(node,elem,p,recoverMethod)
%% recoverP
% Recover the pressure from barycenter to the node.
% The recover methods
% 1) 'LS': least square
% 2) 'LA': solve the Laplacian problem, only this method
%           the new node and new elem will be constructed.
%
% Created by Lin Zhong April, 2013.

if ~exist('recoveryMethod','var')
    recoverMethod = 'LA';
end
NT = size(elem,1);
N = size(node,1);
rp = zeros(N,1);
% baycenter
xnode = 1/3*(node(elem(:,1),:) + node(elem(:,2),:) + node(elem(:,3),:));
NT2N = sparse(repmat(1:NT,1,3),[elem(:,1);elem(:,2);elem(:,3)],true,NT,N);

if strcmp(recoverMethod,'LS')
    % least square fit for every node
    for i = 1:N
        tempx = xnode(NT2N(:,i),:);
        tempp = p(NT2N(:,i));
        tempn = size(tempx,1);
        if tempn == 1
            rp(i) = tempp;
        elseif tempn == 2
            rp(i) = mean(tempp);
        else
            X = ones(tempn,3);
            X(:,1:2) = tempx;
            coefficient = (X'*X)\(X'*tempp);
            rp(i) = node(i,:)*coefficient(1:2) +coefficient(3);
        end
    end
end

if strcmp(recoverMethod,'LA')
    T = auxstructure(elem);
    edge = T.edge;
    edge2elem = T.edge2elem;
    %% find the boundary edge
    isbdEdge = (edge2elem(:,1) == edge2elem(:,2));
    freeEdgeidx = find(~isbdEdge);
    bdEdge = edge(isbdEdge,:);
    freeEdge = edge(freeEdgeidx,:);

    NE = size(edge,1);
    NEbd = size(bdEdge,1);
    NEin = NE - NEbd;
    NTnew = 2*NEin;
    elemnew = zeros(NTnew,3);
    nodenew = [node; xnode];
    %% find the corner point
    acubdNode = accumarray([bdEdge(:,1);bdEdge(:,2)], 1, [N 1]);
    bdNodeIdx = find(acubdNode);
    freeNodeIdx = find(~acubdNode);
    bdNode = node(bdNodeIdx);
    NbdNode = size(bdNode,1);

    % new elems corresponding to the boundary edges
%     elemnew(1:NEbd,:) = [bdEdge(:,1) bdEdge(:,2) N+edge2elem(isbdEdge,1)];

    % new elems corresponding to the interior edges
    elemnew(1:NEin,:) = [freeEdge(:,1) N+edge2elem(freeEdgeidx,1) N+edge2elem(freeEdgeidx,2)];
    elemnew(NEin+1:end,:) = [freeEdge(:,2) N+edge2elem(freeEdgeidx,1) N+edge2elem(freeEdgeidx,2)];

%   A = assemblematrix(nodenew,elemnew,1);
    %------- construc stiffness matrix-------------
    A11 = sparse(N,N);
    A12 = sparse(N,NT);
    [Dlambda,area] = gradbasis(nodenew,elemnew);
    % the original node
    ii = double(elemnew(:,1));
    DiDi =area.*dot(Dlambda(:,:,1),Dlambda(:,:,1),2);
    A11 = A11 +sparse(ii,ii,DiDi,N,N);
    % the original node and the new nodes
    for j = 2:3
        jj = double(elemnew(:,j)-N);
        DiDj =area.*dot(Dlambda(:,:,1),Dlambda(:,:,j),2);
        A12 = A12 +sparse(ii,jj,DiDj,N,NT);
    end
    A = [A11 A12];
    %------------------------------------------
    rp(freeNodeIdx) = (-A(freeNodeIdx,N+1:end)*p)./diag(A(freeNodeIdx,freeNodeIdx));

    % use least square to recover the boundary nodes
    for i = 1:NbdNode
        ii = bdNodeIdx(i);
        tempx = xnode(NT2N(:,ii),:);
        tempp = p(NT2N(:,ii));
        tempn = size(tempx,1);
        if tempn == 1
            rp(ii) = tempp;
        elseif tempn == 2
            rp(ii) = mean(tempp);
        else
            X = ones(tempn,3);
            X(:,1:2) = tempx;
            coefficient = (X'*X)\(X'*tempp);
            rp(ii) = node(ii,:)*coefficient(1:2) +coefficient(3);
        end
    end
end
end


function [Yl,Yh,Yscale] = dtwavexfm2(X,nlevels,biort,qshift);

% Function to perform a n-level DTCWT-2D decompostion on a 2D matrix X
%
% [Yl,Yh,Yscale] = dtwavexfm2(X,nlevels,biort,qshift);
%
%     X -> 2D real matrix/Image
%
%     nlevels -> No. of levels of wavelet decomposition
%
%     biort ->  'antonini'   => Antonini 9,7 tap filters.
%               'legall'     => LeGall 5,3 tap filters.
%               'near_sym_a' => Near-Symmetric 5,7 tap filters.
%               'near_sym_b' => Near-Symmetric 13,19 tap filters.
%
%     qshift -> 'qshift_06' => Quarter Sample Shift Orthogonal (Q-Shift) 10,10 tap filters,
%                              (only 6,6 non-zero taps).
%               'qshift_a' =>  Q-shift 10,10 tap filters,
%                              (with 10,10 non-zero taps, unlike qshift_06).
%               'qshift_b' => Q-Shift 14,14 tap filters.
%               'qshift_c' => Q-Shift 16,16 tap filters.
%               'qshift_d' => Q-Shift 18,18 tap filters.
%
%
%     Yl     -> The real lowpass image from the final level
%     Yh     -> A cell array containing the 6 complex highpass subimages for each level.
%     Yscale -> This is an OPTIONAL output argument, that is a cell array containing
%               real lowpass coefficients for every scale.
%
%
% Example: [Yl,Yh] = dtwavexfm2(X,3,'near_sym_b','qshift_b');
% performs a 3-level transform on the real image X using the 13,19-tap filters
% for level 1 and the Q-shift 14-tap filters for levels >= 2.
%
% Nick Kingsbury and Cian Shaffrey
% Cambridge University, Sept 2001

if isstr(biort) & isstr(qshift)		%Check if the inputs are strings
   biort_exist = exist([biort '.mat']);
   qshift_exist = exist([qshift '.mat']);
   if biort_exist == 2 & qshift_exist == 2;        		%Check to see if the inputs exist as .mat files
      load (biort);
      load (qshift);
   else
      error('Please enter the correct names of the Biorthogonal or Q-Shift Filters, see help DTWAVEXFM2 for details.');
   end
else
   error('Please enter the names of the Biorthogonal or Q-Shift Filters as shown in help DTWAVEXFM2.');
end

orginal_size = size(X);

if ndims(X) >= 3;
   error(sprintf('The entered image is %dx%dx%d, please enter each image slice separately.',orginal_size(1),orginal_size(2),orginal_size(3)));
end

% The next few lines of code check to see if the image is odd in size, if so an extra ...
% row/column will be added to the bottom/right of the image
initial_row_extend = 0;  %initialise
initial_col_extend = 0;
if any(rem(orginal_size(1),2)), %if sx(1) is not divisable by 2 then we need to extend X by adding a row at the bottom
   X = [X; X(end,:)];           %Any further extension will be done in due course.
   initial_row_extend = 1;
end
if any(rem(orginal_size(2),2)), 	%if sx(2) is not divisable by 2 then we need to extend X by adding a col to the left
   X = [X X(:,end)];          %Any further extension will be done in due course.
   initial_col_extend = 1;
end
extended_size = size(X);

if nlevels == 0, return; end

%initialise
Yh=cell(nlevels,1);
if nargout == 3
   Yscale=cell(nlevels,1);   %this is only required if the user specifies a third output component.
end

S = [];
sx = size(X);
if nlevels >= 1,

   % Do odd top-level filters on cols.
   Lo = colfilter(X,h0o).';
   Hi = colfilter(X,h1o).';

   % Do odd top-level filters on rows.
   LoLo = colfilter(Lo,h0o).';			% LoLo
   Yh{1} = zeros([size(LoLo)/2  6]);
   Yh{1}(:,:,[1 6]) = q2c(colfilter(Hi,h0o).');			% Horizontal pair
   Yh{1}(:,:,[3 4]) = q2c(colfilter(Lo,h1o).');			% Vertical pair
   Yh{1}(:,:,[2 5]) = q2c(colfilter(Hi,h1o).');	      % Diagonal pair
   S = [ size(LoLo) ;S];
   if nargout == 3
      Yscale{1} = LoLo;
   end
end

if nlevels >= 2;
   for level = 2:nlevels;
      [row_size col_size] = size(LoLo);
      if any(rem(row_size,4)),		% Extend by 2 rows if no. of rows of LoLo are divisable by 4;
         LoLo = [LoLo(1,:); LoLo; LoLo(end,:)];
      end
      if any(rem(col_size,4)),		% Extend by 2 cols if no. of cols of LoLo are divisable by 4;
         LoLo = [LoLo(:,1)  LoLo  LoLo(:,end)];
      end

      % Do even Qshift filters on rows.
      Lo = coldfilt(LoLo,h0b,h0a).';
      Hi = coldfilt(LoLo,h1b,h1a).';

      % Do even Qshift filters on columns.
      LoLo = coldfilt(Lo,h0b,h0a).';	%LoLo
      Yh{level} = zeros([size(LoLo)/2  6]);
      Yh{level}(:,:,[1 6]) = q2c(coldfilt(Hi,h0b,h0a).');	% Horizontal
      Yh{level}(:,:,[3 4]) = q2c(coldfilt(Lo,h1b,h1a).');	% Vertical
      Yh{level}(:,:,[2 5]) = q2c(coldfilt(Hi,h1b,h1a).');	% Diagonal
      S = [ size(LoLo) ;S];
      if nargout == 3
         Yscale{level} = LoLo;
      end
   end
end

Yl = LoLo;

if initial_row_extend == 1 & initial_col_extend == 1;
   warning(sprintf(' \r\r The image entered is now a %dx%d NOT a %dx%d \r The bottom row and rightmost column have been duplicated, prior to decomposition. \r\r ',...
      extended_size(1),extended_size(2),orginal_size(1),orginal_size(2)));
end

if initial_row_extend == 1 ;
   warning(sprintf(' \r\r The image entered is now a %dx%d NOT a %dx%d \r Row number %d has been duplicated, and added to the bottom of the image, prior to decomposition. \r\r',...
      extended_size(1),extended_size(2),orginal_size(1),orginal_size(2),orginal_size(1)));
end

if initial_col_extend == 1;
   warning(sprintf(' \r\r The image entered is now a %dx%d NOT a %dx%d \r Col number %d has been duplicated, and added to the right of the image, prior to decomposition. \r\r',...
      extended_size(1),extended_size(2),orginal_size(1),orginal_size(2),orginal_size(2)));
end
return

%==========================================================================================
%						**********  	INTERNAL FUNCTION    **********
%==========================================================================================

function z = q2c(y)

% function z = q2c(y)
% Convert from quads in y to complex numbers in z.

sy = size(y);
t1 = 1:2:sy(1); t2 = 1:2:sy(2);
j2 = sqrt([0.5 -0.5]);

% Arrange pixels from the corners of the quads into
% 2 subimages of alternate real and imag pixels.
%  a----b
%  |    |
%  |    |
%  c----d

% Combine (a,b) and (d,c) to form two complex subimages.
p = y(t1,t2)*j2(1) + y(t1,t2+1)*j2(2);     % p = (a + jb) / sqrt(2)
q = y(t1+1,t2+1)*j2(1) - y(t1+1,t2)*j2(2); % q = (d - jc) / sqrt(2)

% Form the 2 subbands in z.
z = cat(3,p-q,p+q);

return


function p = cycle_to_perm ( n, ncycle, t, index )

%*****************************************************************************80
%
%% CYCLE_TO_PERM converts a permutation from cycle to array form.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the number of items permuted.
%    N must be positive.
%
%    Input, integer NCYCLE, the number of cycles.
%    1 <= NCYCLE <= N.
%
%    Input, integer T(N), INDEX(NCYCLE), describes the permutation
%    as a collection of NCYCLE cycles.  The first cycle is
%    T(1) -> T(2) -> ... -> T(INDEX(1)) -> T(1).
%
%    Output, integer P(N), describes the permutation using a
%    single array.  For each index I, I -> P(I).
%

%
%  Check.
%
  ierror = cycle_check ( n, ncycle, t, index );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CYCLE_TO_PERM - Fatal error!\n' );
    fprintf ( 1, '  The input array is illegal.\n' );
    fprintf ( 1, '  IERROR = %d\n', ierror );
    error ( 'CYCLE_TO_PERM - Fatal error!' );
  end

  jhi = 0;

  for i = 1 : ncycle

    jlo = jhi + 1;
    jhi = jhi + index(i);

    for j = jlo : jhi

      if ( j < jhi )
        p(t(j)) = t(j+1);
      else
        p(t(j)) = t(jlo);
      end

    end

  end

  return
end


% show the eigenface images
% examples are conducted in Yale face database
[vectors,Training_matrix,Y]=train(20);  % 10 is the number of eigenvectors
% the size of vectors is 77760*10
for k=1:20
    col=vectors(:,k);  %pick up every column vector
    im=reshape(col,243,320); % transform this vector into 2D-image
    m_a=max(max(im));
    n_b=min(min(im));
    for i=1:243
       for j=1:320
       im(i,j)=(im(i,j)-n_b)/(m_a-n_b)*255;
       end
    end
    subplot(4,5,k);
    imshow(uint8(im));
end


function op = prox_dualize( dualProx, NEG )
%PROX_DUALIZE   Define a proximity function by its dual
%    OP = PROX_DUALIZE( dualOp ) returns an operator implementing the
%    dual of the function dualProx. You can verify they are duals
%    via test_proxPair( dualOp, OP ).
%
%    OP = PROX_DUALIZE( dualOp, 'neg' )
%    OP = PROX_DUALIZE( dualOp, 'negative' )
%       will return the scaled dual of dualOp; that is,
%       dualOp(x) and OP(-x) are duals.
%       The negative is useful because this is the version TFOCS
%       expects for the SCD formulation.
%       For 1-homogenous functions (e.g. norms), this has no effect,
%       since ||x|| = ||-x||.
%
% Warning: if you can calculate the dual function explicitly,
%   it is likely more computationally efficient to do so, rather
%   than rely on this code. This code requires some tricks, some
%   of which can sometimes be expensive; also, it will call dualOp
%   so it is at least as slow as dualOp; and it may require high
%   precision from dualOp. This code can break down if dualOp
%   is not numerically stable.

if nargin < 2, NEG = ''; end
if strcmpi(NEG,'neg') || strcmpi(NEG,'negative')
    op = @(varargin)dualize(dualProx,-1,varargin{:} );
else
    op = @(varargin)dualize(dualProx,1,varargin{:} );
end

function [ v, x ] = dualize( dualProx, scale, x, t )
vec     = @(x) x(:);
myDot   = @(x,y) x(:)'*y(:);
if scale == -1
    x   = -x;
end
switch nargin,
    case 3
        if nargout == 2,
            error( 'This function is not differentiable.'  );
        else
            % This case is a bit tricky...
            %   If the function is non-differentiable, then standard exact
            %   penalty function results tell us that for a sufficiently
            %   small stepsize, we can remove the effect of smoothing.
            %   In other cases, we don't have an exact value, but we hope this
            %   is a reasonable approximation.

%             t       = 1e-15;
%             [~,x2]  = dualProx( x/t, 1/t );
%             v       = myDot(x,x2) - dualProx( x2 );

            % However, some functions break down when 1/t is huge
            % So we will slowly decrease it
            vOld    = Inf;
            t       = 1e-5;
            ok      = false;
            iter    = 0;
            while ~ok && t > eps
                [~,x2]  = dualProx( x/t, 1/t );
                v       = myDot(x,x2) - dualProx( x2 );
                if abs(v-vOld)/max( 1e-10, abs(v) ) < 1e-4
                    % due to exact penalty, we expect that
                    %   for t < t_cutoff, v=vOld up to machine accuracy
                    ok = true;
                else
                    t   = t/10;
                    vOld = v;
                    iter = iter + 1;
                    %fprintf('%d and v is %.2e\n', iter, v );
                end
            end

        end

    case 4
        % This is exact.
        [ignore,x2]  = dualProx( x/t, 1/t );
        x1      = x - t*x2; % Moreau's identity, equation (8.1) in the user guide
        v       = myDot(x1,x2) - dualProx( x2 );

        % If we think it is an indicator function, then round down to zero:
        if abs(v) < 100*eps,
            v  = 0;
        end
        x   = scale*x1;

    otherwise,
        error( 'Not enough arguments.' );
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


%%*************************************************************************
%% mybicgstab
%%
%% [xx,resnrm,flag] = mybicgstab(A,b,M1,tol,maxit)
%%
%% iterate on  bb - (M1)*AA*x
%%
%% r = b-A*xtrue;
%%
%%*************************************************************************

function [xx,resnrm,flag] = mybicgstab(A,b,M1,tol,maxit,printlevel)

N = length(b);
if (nargin < 6); printlevel = 1; end
if (nargin < 5) || isempty(maxit); maxit = max(30,length(A.mat22)); end;
if (nargin < 4) || isempty(tol); tol = 1e-10; end;
tolb = min(1e-4,tol*norm(b));
flag = 1;

x = zeros(N,1);
if (norm(x))
    if isstruct(A); r = b-matvec(A,x); else r = b-mexMatvec(A,x); end;
else
    r =b;
end
err = norm(r); resnrm(1) = err;  minresnrm = err; xx = x;
%%if (err < 1e-3*tolb); return; end

omega  = 1.0;
r_tld = r;
%%
%%
%%
breakyes = 0;
smtol = 1e-40;
for iter = 1:maxit,

    rho   = (r_tld'*r);
    if (abs(rho) < smtol)
        flag = 2;
        if (printlevel); fprintf('*'); end;
        breakyes = 1;
        break;
    end
    if (iter > 1)
        beta  = (rho/rho_1)* (alp/omega);
        p = r + beta*(p - omega*v);
    else
        p = r;
    end
    p_hat = precond(A,M1,p);
    if isstruct(A); v = matvec(A,p_hat); else v = mexMatvec(A,p_hat); end;
    alp = rho / (r_tld'*v);
    s = r - alp*v;
    %%
    s_hat = precond(A,M1,s);
    if isstruct(A); t = matvec(A,s_hat); else t = mexMatvec(A,s_hat); end;
    omega = (t'*s) / (t'*t);
    x = x + alp*p_hat + omega*s_hat;
    r = s - omega*t;
    rho_1 = rho;
    %%
    %% check convergence
    %%
    err = norm(r); resnrm(iter+1) = err; %#ok
    if (err < minresnrm);
        xx = x; minresnrm = err;
    end
    if (err < tolb)
        break;
    end
    if (err > 10*minresnrm)
        if (printlevel); fprintf('^'); end
        breakyes = 2;
        break;
    end
    if (abs(omega) < smtol)
        flag = 2;
        if (printlevel); fprintf('*'); end
        breakyes = 1;
        break;
    end
end
if (~breakyes) && (printlevel >=3); fprintf(' '); end
%%
%%*************************************************************************
%%*************************************************************************
%% precond:
%%*************************************************************************

function Mx = precond(A,L,x)

m = L.matdim; m2 = length(x)-m;
Mx = zeros(length(x),1);

for iter = 1
    if norm(Mx); r = x - matvec(A,Mx); else r = x; end
    if (m2 > 0)
        r1 = full(r(1:m));
    else
        r1 = full(r);
    end
    if (m2 > 0)
        r2 = r(m+1:m+m2);
        w = linsysolvefun(L,r1);
        z = mexMatvec(A.mat12,w,1) - r2;
        z = L.Mu \ (L.Ml \ (L.Mp*z));
        r1 = r1 - mexMatvec(A.mat12,z);
    end
    d = linsysolvefun(L,r1);
    if (m2 > 0)
        d = [d; z]; %#ok
    end
    Mx = Mx + d;
end
%%*************************************************************************
%%*************************************************************************
%% matvec: matrix-vector multiply.
%% matrix = [A.mat11, A.mat12; A.mat12', A.mat22]
%%*************************************************************************

function Ax = matvec(A,x)

m = length(A.mat11); m2 = length(x)-m;
if issparse(x); x = full(x); end
if (m2 > 0)
    x1 = x(1:m);
else
    x1 = x;
end
Ax = mexMatvec(A.mat11,x1);
if (m2 > 0)
    x2 = x(m+1:m+m2);
    Ax = Ax + mexMatvec(A.mat12,x2);
    Ax2 = mexMatvec(A.mat12,x1,1) + mexMatvec(A.mat22,x2);
    Ax = [full(Ax); full(Ax2)];
end
%%*************************************************************************


function h = hs71H(x,lambda)

h = [ 2*x(4)             x(4)   x(4)   2*x(1)+x(2)+x(3);
      x(4)               0      0   x(1);
      x(4)               0      0   x(1);
      2*x(1)+x(2)+x(3)  x(1)  x(1)  0 ];

h = h + lambda.ineqnonlin*-[    0      x(3)*x(4) x(2)*x(4) x(2)*x(3);
                            x(3)*x(4)     0     x(1)*x(4) x(1)*x(3);
                            x(2)*x(4) x(1)*x(4)     0     x(1)*x(2);
                            x(2)*x(3) x(1)*x(3) x(1)*x(2)     0  ];

h = h + lambda.eqnonlin * diag([2 2 2 2]);


%MDL_MICO Create model of Kinova Mico manipulator
%
% MDL_MICO is a script that creates the workspace variable mico which
% describes the kinematic characteristics of a Kinova Mico manipulator
% using standard DH conventions.
%
% Also define the workspace vectors:
%   qz         zero joint angle configuration
%   qr         vertical 'READY' configuration
%
% Reference::
% - "DH Parameters of Mico" Version 1.0.1, August 05, 2013.
%   Kinova
%
% Notes::
% - SI units of metres are used.
% - Unlike most other mdl_xxx scripts this one is actually a function that
%   behaves like a script and writes to the global workspace.
%
% See also Revolute, mdl_jaco, mdl_puma560, mdl_twolink, SerialLink.

% MODEL: Kinova, Mico, 6DOF, standard_DH

% 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

function r = mdl_mico()

    deg = pi/180;

    % robot length values (metres)  page 4
    D1 = 0.2755;
    D2 = 0.2900;
    D3 = 0.1233;
    D4 = 0.0741;
    D5 = 0.0741;
    D6 = 0.1600;
    e2 = 0.0070;

    % alternate parameters
    aa = 30*deg;
    ca = cos(aa);
    sa = sin(aa);
    c2a = cos(2*aa);
    s2a = sin(2*aa);
    d4b = D3 + sa/s2a*D4;
    d5b = sa/s2a*D4 + sa/s2a*D5;
    d6b = sa/s2a*D5 + D6;

    % and build a serial link manipulator

    % offsets from the table on page 4, "Mico" angles are the passed joint
    % angles.  "DH Algo" are the result after adding the joint angle offset.

    robot = SerialLink([
        Revolute('alpha', pi/2,  'a', 0,  'd', D1,   'flip')
        Revolute('alpha', pi,    'a', D2, 'd', 0,    'offset', -pi/2)
        Revolute('alpha', pi/2,  'a', 0,  'd', -e2,  'offset', pi/2)
        Revolute('alpha', 2*aa,  'a', 0,  'd', -d4b)
        Revolute('alpha', 2*aa,  'a', 0,  'd', -d5b, 'offset', -pi)
        Revolute('alpha', pi,    'a', 0,  'd', -d6b, 'offset', pi/2)
        ], ...
        'name', 'Mico', 'manufacturer', 'Kinova');

    %{
        % MDH version, no test  yet
    robot = SerialLink([
        Revolute('alpha', 0,     'a', 0,  'd', D1,   'modified', 'flip')
        Revolute('alpha', -pi/2, 'a', 0,  'd', 0,    'modified', 'offset', -pi/2)
        Revolute('alpha', 0,     'a', D2, 'd', e2,  'modified',  'offset', pi/2)
        Revolute('alpha', -pi/2, 'a', 0,  'd', d4b, 'modified')
        Revolute('alpha', 2*aa,  'a', 0,  'd', d5b, 'modified',  'offset', -pi)
        Revolute('alpha', 2*aa,  'a', 0,  'd', d6b, 'modified',  'offset', pi/2)
        ], ...
        'name', 'Mico', 'manufacturer', 'Kinova');
    %}

    % place the variables into the global workspace
    if nargin == 1
        r = robot;
    elseif nargin == 0
        assignin('caller', 'mico', robot);
        assignin('caller', 'qz', [0 0 0 0 0 0]); % zero angles, arm up
        assignin('caller', 'qr', [270 180 180 0 0 180]*deg); % vertical pose as per Fig 2
    end
end


function h = ref_lconv(f,g,ctype)
%REF_LCONV  Reference linear convolution
%   Usage:  h=ref_lconv(f,g)
%
%   PCONV(f,g) computes the linear convolution of f and g.

% AUTHOR: Jordy van Velthoven

Lf = length(f);
Lg = length(g);

Lh = Lf+Lg-1;

f = [f; zeros(Lh - Lf, 1)];
g = [g; zeros(Lh - Lg, 1)];

h = zeros(Lf+Lg-1, 1);

switch(lower(ctype))
	case {'default'}
    for ii = 0 : Lh-1
      for jj = 0 : Lh-1
        h(ii+1)=h(ii+1)+f(jj+1)*g(mod(ii-jj,Lh)+1);
      end
    end
  case {'r'}
    for ii=0:Lh-1
      for jj=0:Lh-1
	      h(ii+1)=h(ii+1)+f(jj+1)*conj(g(mod(jj-ii, Lh)+1));
      end;
   	end;
  case {'rr'}
    for ii=0:Lh-1
      for jj=0:Lh-1
	      h(ii+1)=h(ii+1)+conj(f(mod(-jj, Lh)+1))*conj(g(mod(jj-ii,Lh)+1));
      end;
    end;
end


% UNPAD_SIGNAL Remove de padding from PAD_SIGNAL
%
% Usage
%    x = UNPAD_SIGNAL(y, resolution, target_sz, center)
%
% Input
%    y (numeric): The signal to be unpadded.
%    resolution (int): The resolution of the signal (as a power of 2), with
%        respect to the original, unpadded version.
%    target_sz (numeric): The size of the original, unpadded version. Combined
%        with resolution, the size of the output y is given by
%        target_sz.*2.*(-resolution).
%    center (boolean, optional): If true, extracts the center part of y, oth-
%        erwise extracts the (upper) left corner (default false).
%
% Output
%    x (numeric): The extracted unpadded signal
%
% Description
%    To handle boundary conditions, a signal is often padded using PAD_SIGNAL
%    before being convolved with CONV_SUB_1D or CONV_SUB_2D. After this, the
%    padding needs to be removed to recover a regular signal. This is achieved
%    using UNPAD_SIGNAL, which takes the padded, convolved signal y as input,
%    as well as its resolution relative to the original, unpadded version,
%    and the size of this original version. Using this, it extracts the
%    coefficients in y that correspond to the domain of the original signal.
%    If the center flag was specified during PAD_SIGNAL, it is specified here
%    again in order to extract the correct part.
%
% See Also
%    PAD_SIGNAL
function x = unpad_signal(x, res, target_sz, center)
    if nargin < 4
        center = 0;
    end

    padded_sz = size(x);

    padded_sz = padded_sz(1:length(target_sz));

    offset = 0.*target_sz;

    if center
        offset = (padded_sz.*2.^res-target_sz)/2;
    end

    offset_ds = floor(offset./2.^res);
    target_sz_ds = 1+floor((target_sz-1)./2.^res);

    switch length(target_sz)
        case 1
            x = x(offset_ds + (1:target_sz_ds),:,:);
        case 2
            x = x(offset_ds(1) + (1:target_sz_ds(1)), ...
                offset_ds(2) + (1:target_sz_ds(2)), :);
    end
end


function variance = normal_01_variance ( )

%*****************************************************************************80
%
%% NORMAL_01_VARIANCE returns the variance of the Normal 01 PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VARIANCE, the variance of the PDF.
%
  variance = 1.0;

  return
end


function bessel_k0_int_values_test ( )

%*****************************************************************************80
%
%% BESSEL_KO_INT_VALUES_TEST demonstrates the use of BESSEL_KO_INT_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BESSEL_KO_INT_VALUES_TEST:\n' );
  fprintf ( 1, '  BESSEL_K0_INT_VALUES stores values of \n' );
  fprintf ( 1, '  the integral of the Bessel function K0.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X           FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = bessel_k0_int_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


function [E,J]=SynthMeasWatsonSHCylSingleRadIsoV_GPD(x, protocol, fibredir, roots)
% Substrate: Impermeable cylinders with one radius in a homogeneous background.
% Orientation distribution: Watson's distribution with SH approximation
% Signal approximation: Gaussian phase distribution.
% Notes: This version includes an isotropic diffusion compartment with its own
% diffusivity.
%
% [E,J]=SynthMeasWatsonSHCylSingleRadIsoV_GPD(x, protocol, fibredir, roots)
% returns the measurements E according to the model and the Jacobian J of the
% measurements with respect to the parameters.  The Jacobian does not
% include derivates with respect to the fibre direction.
%
% x is the list of model parameters in SI units:
% x(1) is the volume fraction of the intracellular space.
% x(2) is the free diffusivity of the material inside and outside the cylinders.
% x(3) is the hindered diffusivity outside the cylinders in perpendicular directions.
% x(4) is the radius of the cylinders.
% x(5) is the concentration parameter of the Watson's distribution.
% x(6) is the volume fraction of the isotropic compartment.
% x(7) is the diffusivity of the isotropic compartment.
%
% protocol is the object containing the acquisition protocol.
%
% fibredir is a unit vector along the symmetry axis of the Watson's
% distribution.  It must be in Cartesian coordinates [x y z]' with size [3 1].
%
% roots contains solutions to the Bessel function equation from function
% BesselJ_RootsCyl.
%
% author: Gary Hui Zhang (gary.zhang@ucl.ac.uk)
%

fiso = x(6);
dIso = x(7);

% Call the model with no isotropic component to get the anisotropic component.
if(nargout == 1)
    Eaniso=SynthMeasWatsonSHCylSingleRadGPD(x, protocol, fibredir, roots);
    Eiso = SynthMeasIsoGPD(dIso, protocol);
else
    [Eaniso,Janiso]=SynthMeasWatsonSHCylSingleRadGPD(x, protocol, fibredir, roots);
    [Eiso, Jiso] = SynthMeasIsoGPD(dIso, protocol);
end

E = (1-fiso)*Eaniso + fiso*Eiso;

if(nargout>1)

    % Update with anisotropic component.
    J = Janiso*(1-fiso);

    % Add derivatives wrt isotropic fraction.
    J(:,6) = Eiso - Eaniso;

    % Add entry for dIso
    J(:,7) = fiso*Jiso;
end


function output = callosqp(interfacedata)

options = interfacedata.options;
model = yalmip2quadprog(interfacedata);

if options.savedebug
    save debugfile model
end

if options.showprogress;showprogress(['Calling ' interfacedata.solver.tag],options.showprogress);end

% Define QP
n_var = length(model.c);
P = model.Q;
q = model.c;
eye_n = speye(n_var);
A = [model.Aeq;model.A; eye_n];
l = full([model.beq; -inf(length(model.b),1); model.lb]);
u = full([model.beq; model.b; model.ub]);

% Define verbose option
options.osqp.verbose = options.verbose;

% Solve with OSQP
OSQPSolver = osqp;
OSQPSolver.setup(P, q, A, l, u, options.osqp);
results = OSQPSolver.solve();

switch results.info.status_val
    case 1
        problem = 0;
    case 2
        problem = 0;
    case -2
        problem = 3;
    case -3
        problem = 1;
    case 3
        problem = 1;
    case -4
        problem = 2;
    case 4
        problem = 2;
    case -5
        problem = 16;
    case -10
        problem = 11;
    otherwise
        problem = -10;
end

% Solver time
solvertime = results.info.run_time;

% Standard interface
Primal      = results.x(:);
Dual        = results.y(1:end-n_var);
infostr     = yalmiperror(problem,interfacedata.solver.tag);
if ~options.savesolverinput
    solverinput = [];
else
    solverinput = model;
end
if ~options.savesolveroutput
    solveroutput = [];
else
    solveroutput = results;
end

% Standard interface
output = createOutputStructure(Primal,Dual,[],problem,infostr,solverinput,solveroutput,solvertime);


function [Mz_z,Mz_xy,F,ref_eff,Mx_xy,My_xy]=simRf(rf,rephase_factor,prephase_factor)
%simRf Simulate an RF pulse with the given pulse shape.
%   [Mz_z,Mz_xy,F,ref_eff,Mx_xy,My_xy]=simRf(pulse,prephase_factor,rephase_factor)
%   Performs a rapid RF pulse simulation based on the rotation formalism.
%   The algorithm is optimized by using quaternions to represent rotations.
%   The compulsory parameter 'rf' is the Pulseq RF pulse. Optional
%   parameter 'rephase_factor' is needed in several cases e.g. to correclty
%   visualize the phase of the magnetization for slice-selective
%   excitation. Another optional parameter 'prephase_factor' is an
%   experimental parameter useful for simulating refocusing pulses or
%   spoiling needed.
%   Return values:
%     Mz_z,Mz_xy:  z and xy comnponents of the magnetisation after the pulse
%                  assuming the unit magnetization was aligned with z before
%                  the pulse. Useful for assessing excitation RF pulses.
%     F:           frequency axis in Hz
%     ref_eff:     Refocusing efficiency of the pulse as a complex value.
%                  Magnitude of ref_eff seems to closely follow Mz_z. Phase
%                  of ref_eff is related to the effective phase of the RF
%                  pulse, e.g. the axis of the planar flip.
%     Mx_xy,My_xy: xy magnetizations after the RF pulse assuming the unit
%                  magnetization was aligned with x or y axis prior to the
%                  pulse, respectively. Useful for detailed analyses of
%                  refocusing pulses.
%
%   The implementation was inspired by the example by Dr. Tony Stoecker
%   (https://github.com/stoeckert/mr-simu-example-ismrm19)
%   The algorithm was rewritten to quaternions and vectorized for
%   performance by MZ
%

bw_mul=4;    % simulation bandwidth (multiplier of the pulse bandwidth)
df=1;        % spectral resolution [Hz]
dt=10e-6;    % (re-)sampling interval

if nargin < 2
    if isfield(rf,'use') && strcmp(rf.use,'refocusing')
        rephase_factor = 0;
    else
        rephase_factor = -(rf.shape_dur-mr.calcRfCenter(rf))/rf.shape_dur;
    end
end

if nargin < 3
    prephase_factor = 0;
end

[bw,f0,spectrum,FF,rfs,tt]=mr.calcRfBandwidth(rf,0.5,df*10,dt);

T     = (1:round(rf.shape_dur/dt))*dt-0.5*dt;                           % timesteps axis [s]
F     = 2*pi*linspace(f0-bw_mul*bw/2,f0+bw_mul*bw/2,bw/df)';               % offset frequencies [rad/s]

shapea = interp1(rf.t, 2*pi*rf.signal.*exp(1i*(rf.phaseOffset+2*pi*rf.freqOffset*rf.t)),T,'linear',0);

% intialize result vectors
M_ROT=zeros(size(F));
Z_ROT=zeros(size(F));
sf=size(F);
q=zeros(sf(1),4);
q(:,1)=1; % init rotation quaternions

% prephaser / left spoiler
W = -F*dt*length(T)*prephase_factor;     % effective field rotation angle
Q = [cos(W/2) zeros(sf) zeros(sf) sin(W/2)];
q=quat_multiply(q,Q);

% RF pulse simulation
for j=1:length(T)
    W = -dt*sqrt(abs(shapea(j))^2+F.^2); % effective field rotation angles
    n = dt * [real(shapea(j))*ones(sf) imag(shapea(j))*ones(sf) F]./abs(W); % effective field rotation axes
    Q = [cos(W/2) sin(W/2).*n];
    q=quat_multiply(q,Q);
end

% rephaser / right spoiler / refocusing pulse
W = -F*dt*length(T)*rephase_factor;     % effective field rotation angle
Q = [cos(W/2) zeros(sf) zeros(sf) sin(W/2)];
q=quat_multiply(q,Q);

% export results
F=F/(2*pi);
m=zeros(sf(1),4);

% excitation: start with M0=M_z
m(:,4)=1;
m0rf=quat_multiply(quat_conj(q),quat_multiply(m,q));
Mz_z=m0rf(:,4);
Mz_xy=m0rf(:,2)+1i*m0rf(:,3);

% refocusing: start both with M0=M_x and them M0=M_y
m=zeros(sf(1),4);
m(:,2)=1;
Mx_xy=quat_multiply(quat_conj(q),quat_multiply(m,q));
Mx_xy=Mx_xy(:,2)+1i*Mx_xy(:,3);
m=zeros(sf(1),4);
m(:,3)=1;
My_xy=quat_multiply(quat_conj(q),quat_multiply(m,q));
My_xy=My_xy(:,2)+1i*My_xy(:,3);
ref_eff=(Mx_xy+My_xy*1i)/2;
end

function qout = quat_multiply( q, r )
%  quat_multiply: Calculate the product of two quaternions.

% Calculate vector portion of quaternion product
% vec = s1*v2 + s2*v1 + cross(v1,v2)
vec = [q(:,1).*r(:,2) q(:,1).*r(:,3) q(:,1).*r(:,4)] + ...
         [r(:,1).*q(:,2) r(:,1).*q(:,3) r(:,1).*q(:,4)]+...
         [ q(:,3).*r(:,4)-q(:,4).*r(:,3) ...
           q(:,4).*r(:,2)-q(:,2).*r(:,4) ...
           q(:,2).*r(:,3)-q(:,3).*r(:,2)];

% Calculate scalar portion of quaternion product
% scalar = s1*s2 - dot(v1,v2)
scalar = q(:,1).*r(:,1) - q(:,2).*r(:,2) - ...
             q(:,3).*r(:,3) - q(:,4).*r(:,4);

qout = [scalar  vec];
end

function q = quat_conj( q )
%  quat_conj Calculate the conjugate of a quaternion.
q(:,2:4) = -q(:,2:4);
end


% %
% Setup default values and run PK computation. Note that the kernel will
% perform BETTER if you learn the parameters via cross validation instead of
% using these defaults.
%
% Marion Neumann (m.neumann@wustl.edu)
% %

% propagation kernel parameter
num_iterations = 3;     % number of iterations (sth small 2 or 3)

% hashing parameters
w              = 1e-5;  % bin width
distance       = 'tv';  % distance to approximately preserve

% load you dataset HERE
load('mutag_mat');

num_nodes   = size(A, 1);
num_classes = max(labels);

initial_label_distributions = accumarray([(1:num_nodes)', labels], 1, [num_nodes, num_classes]);

% create a function handle to a feature transformation. Here we will
% use label diffustion as we have fully labeled graphs.
transformation = @(features) label_diffusion(features, A);

% calculate the graph kernel using the default (linear) base kernel
K = propagation_kernel(initial_label_distributions, graph_ind, transformation, ...
                       num_iterations, ...
                       'distance', distance, ...
                       'w',        w);

% % If you want to set the BASE KERNEL to an RBF kernel instead of the default
% % linear base kernel, use the following.
% length_scale = 3;
% base_kernel = @(counts) ...
%               exp(-(squareform(pdist(counts)).^2 / (2 * length_scale^2)));
%
%           % calculate the graph kernel again using the new parameters
% K = propagation_kernel(initial_label_distributions, graph_ind, transformation, ...
%                        num_iterations, ...
%                        'distance',    distance, ...
%                        'w',           w, ...
%                        'base_kernel', base_kernel);


% Two examples to show how to solve an MILP and an LP with interior method

disp('-- Integer problem --');
s=1;
c=[-1,-1]';
a=[-2,5;2,-2];
b=[5;1];
ctype=['U','U']';
lb=[0;0]; ub=[];
vartype=['B';'B'];
param.msglev=3;
[xmin,fmin,status,extra]=glpk(c,a,b,lb,ub,ctype,vartype,s,param)
% --- OBSOLETE ---
% [xmin,fmin,status,extra]=glpkmex(s,c,a,b,ctype,lb,ub,vartype,param)
pause;

disp('3rd problem');
s=1;
c=[0 0 0 -1 -1]';
a=[-2 0 0 1 0;...
    0 1 0 0 2;...
    0 0 1 3 2];
b=[4 12 18]';
ctype=['S','S','S']';
lb=[0,0,0,0,0]'; ub=[];
vartype=['C','C','C','C','C']';
param.lpsolver=2;
[xmin,fmin,status,extra]=glpk(c,a,b,lb,ub,ctype,vartype,s,param)
% --- OBSOLETE ---
% [xmin,fmin,status,extra]=glpkmex(s,c,a,b,ctype,lb,ub,vartype)


clc;
clear all;
close all;

file = 'data.csv'; % Dataset

% Reading training file
data = dlmread(file);
label = data(:,end);

% Extracting positive data points
idx = (label==1);
pos_data = data(idx,:);
row_pos = size(pos_data,1);

% Extracting negative data points
neg_data = data(~idx,:);
row_neg = size(neg_data,1);

% Random permuation of positive and negative data points
p = randperm(row_pos);
n = randperm(row_neg);

% 80-20 split for training and test
tstpf = p(1:round(row_pos/5));
tstnf = n(1:round(row_neg/5));
trpf = setdiff(p, tstpf);
trnf = setdiff(n, tstnf);

train_data = [pos_data(trpf,:);neg_data(trnf,:)];
test_data = [pos_data(tstpf,:);neg_data(tstnf,:)];

% Decision Tree
prediction = SMOTEBoost(train_data,test_data,'tree',false);
disp ('    Label   Probability');
disp ('-----------------------------');
disp (prediction);


% Version 1.000
%
% Code provided by Ruslan Salakhutdinov and Geoff Hinton
%
% Permission is granted for anyone to copy, use, modify, or distribute this
% program and accompanying programs and documents for any purpose, provided
% this copyright notice is retained and prominently displayed, along with
% a note saying that the original programs are available from our
% web page.
% The programs and documents are distributed without any warranty, express or
% implied.  As the programs were written for research purposes only, they have
% not been tested to the degree that would be advisable in any important
% application.  All use of these programs is entirely at the user's own risk.

function [f, df] = CG_MNIST(VV,Dim,XX);

l1 = Dim(1);
l2 = Dim(2);
l3 = Dim(3);
l4= Dim(4);
l5= Dim(5);
l6= Dim(6);
l7= Dim(7);
l8= Dim(8);
l9= Dim(9);
N = size(XX,1);

% Do decomversion.
 w1 = reshape(VV(1:(l1+1)*l2),l1+1,l2);
 xxx = (l1+1)*l2;
 w2 = reshape(VV(xxx+1:xxx+(l2+1)*l3),l2+1,l3);
 xxx = xxx+(l2+1)*l3;
 w3 = reshape(VV(xxx+1:xxx+(l3+1)*l4),l3+1,l4);
 xxx = xxx+(l3+1)*l4;
 w4 = reshape(VV(xxx+1:xxx+(l4+1)*l5),l4+1,l5);
 xxx = xxx+(l4+1)*l5;
 w5 = reshape(VV(xxx+1:xxx+(l5+1)*l6),l5+1,l6);
 xxx = xxx+(l5+1)*l6;
 w6 = reshape(VV(xxx+1:xxx+(l6+1)*l7),l6+1,l7);
 xxx = xxx+(l6+1)*l7;
 w7 = reshape(VV(xxx+1:xxx+(l7+1)*l8),l7+1,l8);
 xxx = xxx+(l7+1)*l8;
 w8 = reshape(VV(xxx+1:xxx+(l8+1)*l9),l8+1,l9);

  XX = [XX ones(N,1)];
  w1probs = 1./(1 + exp(-XX*w1)); w1probs = [w1probs  ones(N,1)];
  w2probs = 1./(1 + exp(-w1probs*w2)); w2probs = [w2probs ones(N,1)];
  w3probs = 1./(1 + exp(-w2probs*w3)); w3probs = [w3probs  ones(N,1)];
  w4probs = w3probs*w4; w4probs = [w4probs  ones(N,1)];
  w5probs = 1./(1 + exp(-w4probs*w5)); w5probs = [w5probs  ones(N,1)];
  w6probs = 1./(1 + exp(-w5probs*w6)); w6probs = [w6probs  ones(N,1)];
  w7probs = 1./(1 + exp(-w6probs*w7)); w7probs = [w7probs  ones(N,1)];
  XXout = 1./(1 + exp(-w7probs*w8));

f = -1/N*sum(sum( XX(:,1:end-1).*log(XXout) + (1-XX(:,1:end-1)).*log(1-XXout)));
IO = 1/N*(XXout-XX(:,1:end-1));
Ix8=IO;
dw8 =  w7probs'*Ix8;

Ix7 = (Ix8*w8').*w7probs.*(1-w7probs);
Ix7 = Ix7(:,1:end-1);
dw7 =  w6probs'*Ix7;

Ix6 = (Ix7*w7').*w6probs.*(1-w6probs);
Ix6 = Ix6(:,1:end-1);
dw6 =  w5probs'*Ix6;

Ix5 = (Ix6*w6').*w5probs.*(1-w5probs);
Ix5 = Ix5(:,1:end-1);
dw5 =  w4probs'*Ix5;

Ix4 = (Ix5*w5');
Ix4 = Ix4(:,1:end-1);
dw4 =  w3probs'*Ix4;

Ix3 = (Ix4*w4').*w3probs.*(1-w3probs);
Ix3 = Ix3(:,1:end-1);
dw3 =  w2probs'*Ix3;

Ix2 = (Ix3*w3').*w2probs.*(1-w2probs);
Ix2 = Ix2(:,1:end-1);
dw2 =  w1probs'*Ix2;

Ix1 = (Ix2*w2').*w1probs.*(1-w1probs);
Ix1 = Ix1(:,1:end-1);
dw1 =  XX'*Ix1;

df = [dw1(:)' dw2(:)' dw3(:)' dw4(:)' dw5(:)' dw6(:)'  dw7(:)'  dw8(:)'  ]';


function P = getFamily(L,families)
[K,n] = size(L);
nf = length(families);
C = zeros(K,nf);
for i=1:nf
    indf = families{i};
    C(indf,i) = 1;
end
f0 = C*sum(C,1)'.^-1/size(C,2);
P = zeros(nf,n);
for i=1:n
    logPm = L(:,i) + log(f0);
    Pm = exp(logPm-max(logPm));
    Pm = Pm./sum(Pm);
    P(:,i) = C'*Pm;
end


function [E,J]=SynthMeasWatsonSHCylSingleRadTortIsoVIsoDot_GPD_B0(x, protocol, fibredir, roots)
% Substrate: Impermeable cylinders with one radius in a homogeneous background.
% Orientation distribution: Watson's distribution with SH approximation
% Signal approximation: Gaussian phase distribution.
% Notes: This version estimates the hindered diffusivity from the free diffusivity
% and packing density using Szafer et al's tortuosity model for randomly
% packed cylinders.
% This version includes an isotropic diffusion compartment with its own
% diffusivity.
% This version includes a stationary water compartment.
% Includes a free parameter for the measurement at b=0.
%
% [E,J]=SynthMeasWatsonSHCylSingleRadTortIsoVIsoDot_GPD_B0(x, protocol, fibredir, roots)
% returns the measurements E according to the model and the Jacobian J of the
% measurements with respect to the parameters.  The Jacobian does not
% include derivates with respect to the fibre direction.
%
% x is the list of model parameters in SI units:
% x(1) is the volume fraction of the intracellular space.
% x(2) is the free diffusivity of the material inside and outside the cylinders.
% x(3) is the radius of the cylinders.
% x(4) is the concentration parameter of the Watson's distribution.
% x(5) is the volume fraction of the isotropic compartment.
% x(6) is the diffusivity of the isotropic compartment.
% x(7) is the volume fraction of the isotropic restriction.
% x(8) is the measurement at b=0.
%
% protocol is the object containing the acquisition protocol.
%
% fibredir is a unit vector along the symmetry axis of the Watson's
% distribution.  It must be in Cartesian coordinates [x y z]' with size [3 1].
%
% roots contains solutions to the Bessel function equation from function
% BesselJ_RootsCyl.
%
% author: Gary Hui Zhang (gary.zhang@ucl.ac.uk)
%

S0 = x(8);

% Call the other function to get normalized measurements.
if(nargout == 1)
    Enorm=SynthMeasWatsonSHCylSingleRadTortIsoVIsoDot_GPD(x, protocol, fibredir, roots);
else
   [Enorm,Jnorm]=SynthMeasWatsonSHCylSingleRadTortIsoVIsoDot_GPD(x, protocol, fibredir, roots);
end

E = Enorm*S0;

if(nargout>1)
    J = Jnorm*S0;
    J(:,8) = Enorm;
end


function op = prox_l1pos( q )
%PROX_L1POS    L1 norm, restricted to x >= 0
%    OP = PROX_L1( q ) implements the nonsmooth function
%        OP(X) = norm(q.*X,1) + indicator_{ X >= 0 }
%    Q is optional; if omitted, Q=1 is assumed. But if Q is supplied,
%    then it must be a positive real scalar (or must be same size as X).

% New in v1.0d

if nargin == 0,
	q = 1;
elseif ~isnumeric( q ) || ~isreal( q ) ||  any( q(:) < 0 ) || all(q(:)==0) %|| numel( q ) ~= 1
	error( 'Argument must be positive.' );
end

op = tfocs_prox( @(x)f(x,q), @(x,t)prox_f(x,t,q), 'vector');
end

function v = f(x,q)
    if any( x(:) < 0 )
        v = Inf;
    elseif isscalar(q)
        v = q*sum( x(:) );
    else
        v = sum( q(:).*x(:) );
    end
end

% The proximity operator is a simplified version of shrinkage:
function x = prox_f(x,t,q)
    x   = max( 0, x - t*q );
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


% Gnufft_test.m
% Test the Gnufft object (vs exact Gdsft)

%% create Gnufft class object
if 1 || ~isvar('A'), printm 'setup Gnufft_test'
	im plc 3 3
	if 1 % 2d
		N = [32 30];
		J = [6 7];
	%	N = [32 1]; J = [5 1];
		omega = linspace(0, 10*2*pi, 201)'; % crude spiral:
		omega = pi*[cos(omega) sin(omega)].*omega(:,[1 1])/max(omega);
		if im, im subplot 1, plot(omega(:,1), omega(:,2), '.'), end
	else % 3d
		N = [16 12 14];
		J = [6 7 5];
		tmp = linspace(0, 10*2*pi, 201)';
		omega = pi*[cos(tmp) sin(tmp)].*tmp(:,[1 1])/max(tmp);
		omega(:,3) = pi * tmp / max(tmp); % spiral cone
		if im, im subplot 1, plot3(omega(:,1), omega(:,2), omega(:,3), '.'), end
	end

	K = 2*N;
	if N(2) == 1
		args = {omega(:,1), N(1), J(1), K(1)}; omega(:,2) = 0;
	else
		args = {omega, N, J, K};
	end

%	mask = true(N); mask(1,1) = false;
% todo: mask!

	args = {args{:}, 'table', 2^10, 'minmax:kb'}; % test with table

	A = Gnufft(args);

	Ad = Gdsft(omega, N);
end

%% test save/load
if 0
	save('/tmp/A.mat', 'A')
return
end

%% test data
if 1 || ~isvar('x'), printm 'setup data'
	if length(N) == 2
		x = zeros(N);
		x(5:25,10:25) = 1;
		x(15:20,15:20) = 2;
		x(15,5) = 2;
		if N(2) == 1
			x = x(:,5,1);
		end
	else
		rng(0)
		x = rand(N);
	end
	im(2, x, '\x')

	yd = Ad * x;

	if length(N) == 2
		n1 = ([0:N(1)-1]/N(1) - 0.5)*2*pi;
		if N(2) == 1 % 1D case
			yd_g = interp1(omega(:,1), yd, n1);
		else
			n2 = ([0:N(2)-1]/N(2) - 0.5)*2*pi;
			[nn1 nn2] = ndgrid(n1, n2);
			yd_g = griddata(omega(:,1), omega(:,2), yd,  nn1, nn2);
			yd_g(isnan(yd_g)) = 0;
		end
		im(3, abs(yd_g), '$|\y_d|$'), cbar
	end
end

%% build gram
if 1, printm 'Gnufft gram'
	wi = [1:size(omega,1)]';
	T = build_gram(A, wi);
	y2 = T * x(:);
	y1 = A' * (wi .* (A * x(:)));
	max_percent_diff y1 y2
%	equivs(y1, y2)
prompt
end

%% compare forward
if 1, printm 'forward'
	yn = A * [x(:) x(:)]; % test with two
	yn = yn(:,1);

	if length(N) == 2
		if N(2) == 1 % 1D case
			yn_g = interp1(omega(:,1), yn, n1);
		else
			yn_g = griddata(omega(:,1), omega(:,2), yn,  nn1, nn2);
			yn_g(isnan(yn_g)) = 0;
		end

		im(4, abs(yn_g), '$|\y_g|$'), cbar
		im(5, abs(yd_g - yn_g), '$|\y_g - \y_d|$'), cbar
	end
	max_percent_diff yd yn
end

%% compare adjoint
if 1, printm 'adjoint'
	yb = ones(size(omega,1), 1);
	xd = Ad' * yb;
	xd = iembed(Ad, xd);
	xn = A' * [yb yb]; % test with two
	xn = xn(:,1);
	xn = iembed(A, xn);

	im(7, fftshift(abs(xd)), '|back dtft|'), cbar
	im(8, fftshift(abs(xn)), '|back nufft|'), cbar
	im(9, fftshift(abs(xn-xd)), '|back err|'), cbar
	max_percent_diff xd xn
end