function [rval_space,rval_time,ind_space,ind_time] = classify_comp_corr(Yr,A,C,b,f,options)

% Classify components by looking at how well the spatial and temporal components
% correlate with the raw data at point of high activation

% INPUTS
% Yr:       reshaped data in 3D matrix or memmaped file
% A,C,b,f:  identified components
% options:  options structure
%    space_thresh:  r-value threshold for spatial components (default: 0.4)
%    time_thresh:  r-value threshold for temporal components (default: 0.4)
%    Athresh:  threshold for determining spatial overlap (default: 0.1)
%    Np:       number of high activity intervals for each component (default: 20)
%    peak_int: Interval around each local peak to be considered (default: -2:6)
%    MinPeakDist:   minimum peak distance for finding points of high activity  (default: 10)

% OUTPUTS:
% rval_space:     r-values between spatial components and data patches
% rval_time:      r-values between temporal components and trace averages
% ind_space:      components with rval_space > space_thresh
% ind_time:       components with rval_time > time_thresh
% MinPeakDist:

% Written by Eftychios A. Pnevmatikakis, Simons Foundation, 2016
% based on discussions with Matt Kaufman and Farzaneh Najafi, CSHL

defoptions = CNMFSetParms;
if nargin < 6 || isempty(options)
    options = defoptions;
end

if ~isfield(options,'peak_int') || isempty(options.peak_int); int = defoptions.peak_int; else int = options.peak_int; end
if ~isfield(options,'Npeaks') || isempty(options.peak_int); Np = defoptions.Npeaks; else Np = options.Npeaks; end
if ~isfield(options,'A_thresh') || isempty(options.A_thresh); Athresh = defoptions.A_thresh; else Athresh = options.A_thresh; end
if ~isfield(options,'space_thresh') || isempty(options.space_thresh); options.space_thresh = defoptions.space_thresh; end
if ~isfield(options,'time_thresh') || isempty(options.time_thresh); options.time_thresh = defoptions.time_thresh; end
if ~isfield(options,'MinPeakDist') || isempty(options.MinPeakDist); options.MinPeakDist = defoptions.MinPeakDist; end

memmaped = isobject(Yr);

[K_m,T] = size(C);
pk = zeros(K_m,Np);

parfor i = 1:K_m
    [~,pk_temp] = findpeaks(C(i,:),'SortStr','descend','Npeaks',Np,'MinPeakDistance',options.MinPeakDist);
    if ~isempty(pk_temp)
        if length(pk_temp) < Np
            pk_temp(length(pk_temp)+1:Np) = pk_temp(1);
        end
        pk(i,:) = pk_temp;
    end
end

%% expand intervals
lint = length(int);

locs = repmat(pk,1,lint) + kron(int,ones(size(pk)));
locs = sort(locs,2,'ascend');
locs(locs<1)=1;
locs(locs>T)=T;
LOCS = mat2cell(locs,ones(K_m,1),Np*lint);
for i = 1:K_m
    LOCS{i} = unique(LOCS{i});
end

%% compute r-values
nA = full(sqrt(sum(A.^2)))';
tAA = (A'*A)./(nA*nA') > Athresh;
tAA(1:K_m+1:K_m^2) = 0;

rval_space = zeros(K_m,1);
rval_time  = zeros(K_m,1);

if options.d3 == 1; b_rs = reshape(b,options.d1,options.d2,[]); else b_rs = reshape(b,options.d1,options.d2,options.d3,[]); end
if memmaped
    parfor i = 1:K_m
        ovlp_cmp = find(tAA(:,i));
        indeces = LOCS{i};
        for j = 1:length(ovlp_cmp)
            indeces = setdiff(indeces,LOCS{ovlp_cmp(j)});
        end
        a_temp = reshape(A(:,i),options.d1,options.d2*options.d3);
        [rows,temp] = find(a_temp>0);
        [cols,plns] = ind2sub([options.d2,options.d3],temp);
        if options.d3 > 1
            a_temp = reshape(full(A(:,i)),options.d1,options.d2,options.d3);
            a_temp = a_temp(min(rows):max(rows),min(cols):max(cols),min(plns):max(plns));
        else
            a_temp = a_temp(min(rows):max(rows),min(cols):max(cols));
        end

        if options.d3 == 1
            b_temp = reshape(b_rs(min(rows):max(rows),min(cols):max(cols),:),numel(a_temp),[]);
        else
            b_temp = reshape(b_rs(min(rows):max(rows),min(cols):max(cols),min(plns):max(plns),:),numel(a_temp),[]);
        end
        if ~isempty(indeces)
            if options.d3 == 1
                yytemp = Yr.Y(min(rows):max(rows),min(cols):max(cols),min(indeces):max(indeces));
                y_temp = yytemp(:,:,indeces-min(indeces)+1);
            else
                yytemp = Yr.Y(min(rows):max(rows),min(cols):max(cols),min(plns):max(plns),min(indeces):max(indeces));
                y_temp = yytemp(:,:,:,indeces-min(indeces)+1);
            end
            y_temp = reshape(y_temp,[],length(indeces));
            mY_space = double(mean(y_temp,2) - b_temp*mean(f(:,indeces),2));
            mY_time = double(mean(y_temp,1)-mean(b_temp,1)*f(:,indeces));
            rval_space(i) = corr(full(a_temp(:)),mY_space);
            rval_time(i) = corr(C(i,indeces)',mY_time');
        else
            rval_space(i) = NaN;
            rval_time(i) = NaN;
        end
    end
else
    parfor i = 1:K_m
        ovlp_cmp = find(tAA(:,i));
        indeces = LOCS{i};
        for j = 1:length(ovlp_cmp)
            indeces = setdiff(indeces,LOCS{ovlp_cmp(j)});
        end
        a_temp = reshape(A(:,i),options.d1,options.d2*options.d3);
        [rows,temp] = find(a_temp>0);
        [cols,plns] = ind2sub([options.d2,options.d3],temp);
        if options.d3 > 1
            a_temp = reshape(full(A(:,i)),options.d1,options.d2,options.d3);
            a_temp = a_temp(min(rows):max(rows),min(cols):max(cols),min(plns):max(plns));
        else
            a_temp = a_temp(min(rows):max(rows),min(cols):max(cols));
        end

        if options.d3 == 1
            b_temp = reshape(b_rs(min(rows):max(rows),min(cols):max(cols),:),numel(a_temp),[]);
        else
            b_temp = reshape(b_rs(min(rows):max(rows),min(cols):max(cols),min(plns):max(plns),:),numel(a_temp),[]);
        end
        if ~isempty(indeces)
            if options.d3 == 1
                y_temp = Yr(min(rows):max(rows),min(cols):max(cols),indeces);
            else
                y_temp = Yr(min(rows):max(rows),min(cols):max(cols),min(plns):max(plns),indeces);
            end
            y_temp = reshape(y_temp,[],length(indeces));
            mY_space = double(mean(y_temp,2) - b_temp*mean(f(:,indeces),2));
            mY_time = double(mean(y_temp,1)-mean(b_temp,1)*f(:,indeces));
            rval_space(i) = corr(full(a_temp(:)),mY_space);
            rval_time(i) = corr(C(i,indeces)',mY_time');
        else
            rval_space(i) = NaN;
            rval_time(i) = NaN;
        end
    end
end

ind_space = rval_space > options.space_thresh;
ind_time = rval_time > options.time_thresh;


function handles = tight_subplots(rows, columns, gap, row_margin, column_margin)
% tight_subplots Initializes a grid of axes
%
% Initializes a grid of axes with adjustable gaps and margins
% and returns their handles.
%
% Credit: Pekka Kumpulainen (2010) @tut.fi Tampere University of Technology / Automation Science and Engineering
%
% Input:
% - rows (integer): Number of axes in hight (vertical direction).
% - columns (integer): Number of axes in width (horizontal direction).
% - gap (double): Gaps between the axes in normalized units (0...1)
%   or [gap_h gap_w] for different gaps in height and width.
% - row_margin (<type>): Margins in height in normalized units (0...1)
%   or [lower upper] for different lower and upper margins.
% - column_margin (<type>): Margins in width in normalized units (0...1)
%   or [left right] for different left and right margins
%
% Output:
% - handles (matrix): Matrix of handles for the axes objects
%   starting from upper left corner, going row-wise as in
%   going row-wise as in.
%

if nargin<3; gap = .02; end
if nargin<4 || isempty(row_margin); row_margin = .05; end
if nargin<5; column_margin = .05; end

if numel(gap)==1;
    gap = [gap gap];
end
if numel(column_margin)==1;
    column_margin = [column_margin column_margin];
end
if numel(row_margin)==1;
    row_margin = [row_margin row_margin];
end

axh = (1-sum(row_margin)-(rows-1)*gap(1))/rows;
axw = (1-sum(column_margin)-(columns-1)*gap(2))/columns;

py = 1-row_margin(2)-axh;

handles = zeros(rows*columns,1);
ii = 0;
for ih = 1:rows
    px = column_margin(1);

    for ix = 1:columns
        ii = ii+1;
        handles(ii) = axes('Units','normalized', ...
            'Position',[px py axw axh], ...
            'XTickLabel','', ...
            'YTickLabel',''); %#ok<LAXES>
        px = px+axw+gap(2);
    end
    py = py-axh-gap(1);
end


function a = band(a,p,q)
%BAND         Extract band from matrix a, lower bandwidth p, upper bandwidth q
%   if parameter q is omitted, q:=p
%
%   c = band(a,p,q)
%

% written  04/04/04     S.M. Rump
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  if nargin<3
    q = p;
  end

  index = ( tril(triu(ones(size(a.x)),-p),q) == 0 );
  a.x(index) = 0;
  a.dx(:,index) = 0;
  a.hx(:,index) = 0;


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

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym asin (@var{x})
%% Symbolic asin function.
%%
%% Example:
%% @example
%% @group
%% syms x
%% y = asin (x)
%%   @result{} y = (sym) asin(x)
%% @end group
%% @end example
%%
%% Note: this file is autogenerated: if you want to edit it, you might
%% want to make changes to 'generate_functions.py' instead.
%%
%% @end defmethod

function y = asin(x)
  if (nargin ~= 1)
    print_usage ();
  end
  y = elementwise_op ('asin', x);
end

%!error asin (sym(1), 2)
%!assert (isequaln (asin (sym(nan)), sym(nan)))

%!shared x, d
%! d = 1;
%! x = sym('1');

%!test
%! f1 = asin(x);
%! f2 = asin(d);
%! assert( abs(double(f1) - f2) < 1e-15 )

%!test
%! D = [d d; d d];
%! A = [x x; x x];
%! f1 = asin(A);
%! f2 = asin(D);
%! assert( all(all( abs(double(f1) - f2) < 1e-15 )))

%!test
%! % round trip
%! y = sym('y');
%! A = asin (d);
%! f = asin (y);
%! h = function_handle (f);
%! B = h (d);
%! assert (A, B, -eps)


function out = deriv(f, xx, varargin)
%DERIV   Evaluate a derivative of a CHEBFUN.
%
%   DERIV(F, X) evaluates the first derivative of the CHEBFUN F at the points in
%   X. If F is a quasimatrix with columns F1, ..., FN, then the result will be
%   [F1(X), ..., FN(X)], the horizontal concatenation of the results of
%   evaluating each column at the points in X.
%
%   DERIV(F, X, M) is the same as above, but returns the values of the Mth
%   derivative of F.
%
%   DERIV(F, X, S) or DERIV(F, X, S, M) where S is one of the strings 'left',
%   'right', '+', or '-', evaluates the left- or right-sided limit as described
%   in chebfun/feval.
%
%   Example 1:
%     u = chebfun(@sin);
%     deriv(u, 1) % u'(1)
%     deriv(u, .5, 2) % u''(.5)
%     deriv(u, 0.1:0.01:0.2) % u'(0.1:0.01:0.2)
%
%   Example 2:
%     x = chebfun('x')
%     u = abs(x);
%     deriv(u, 0, 'left')  % ans = -1
%     deriv(u, 0, 'right') % ans = +1
%
% See also chebfun/diff, chebfun/feval.

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

% The most trivial case:
if ( isempty(f) )
    return
end

% By default, compute first derivative:
m = 1;

% Parse the inputs:
if ( nargin == 3 )
    if ( isnumeric(varargin{1}) ) % DERIV(F, X, M)
        m = varargin{1};
        varargin = {};
    else                          % DERIV(F, X, S)
        m = 1;  % By default, compute first derivative
    end
elseif ( nargin == 4 )            % DERIV(F, X, S, M)
    m = varargin{2};
    varargin{2} = [];
end

if ( m == 0 )
    % Trivial case
    out = feval(f, xx, varargin{:});
else
    out = feval(diff(f, m), xx, varargin{:});
end

end


function out = issmooth(f)
%ISSMOOTH   True if a SINGFUN object F is smooth.
%   ISSMOOTH(F) returns TRUE if the SINGFUN objects F has zero EXPONENTS or if
%   the SMOOTHPART is zero. The test is FALSE otherwise.
%
% See also ISSING.

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

out = all(f.exponents == 0) || all(iszero(f.smoothPart));

end


function pass = test_isequal( pref )

% Grab some preferences
if ( nargin == 0 )
    pref = chebfunpref();
end

% Test with function 1
f = ballfun(ones(21,20,22));
g = f+f-f;

pass(1) = (isequal(f,g) && isequal(g,f));
end


% Plot Simulation and On-board Model Output

sim_INS_Out = get(logsout, 'INS_Out');
ref_INS_Out = get(logsout, 'INS_Out_Ref');

sim_FMS_Out = get(logsout, 'FMS_Out');
ref_FMS_Out = get(logsout, 'FMS_Out_Ref');

sim_Control_Out = get(logsout, 'Control_Out');
ref_Control_Out = get(logsout, 'Control_Out_Ref');

%% Euler
figure;
ax1 = subplot(3,1,1);
plot(sim_INS_Out.Values.phi, 'Color', 'r');
hold on;
plot(ref_INS_Out.Values.phi, 'Color', 'b');
grid on;
legend('\phi', '\phi_r')

ax2 = subplot(3,1,2);
plot(sim_INS_Out.Values.theta, 'Color', 'r');
hold on;
plot(ref_INS_Out.Values.theta, 'Color', 'b');
grid on;
legend('\theta', '\theta_r')

ax3 = subplot(3,1,3);
plot(sim_INS_Out.Values.psi, 'Color', 'r');
hold on;
plot(ref_INS_Out.Values.psi, 'Color', 'b');
grid on;
legend('\psi', '\psi_r')

linkaxes([ax1,ax2,ax3], 'x');

%% Velocity
figure;
ax1 = subplot(3,1,1);
plot(sim_INS_Out.Values.vn, 'Color', 'r');
hold on;
plot(ref_INS_Out.Values.vn, 'Color', 'b');
grid on;
legend('vn', 'vn_r')

ax2 = subplot(3,1,2);
plot(sim_INS_Out.Values.ve, 'Color', 'r');
hold on;
plot(ref_INS_Out.Values.ve, 'Color', 'b');
grid on;
legend('ve', 've_r')

ax3 = subplot(3,1,3);
plot(sim_INS_Out.Values.vd, 'Color', 'r');
hold on;
plot(ref_INS_Out.Values.vd, 'Color', 'b');
grid on;
legend('vd', 'vd_r')

linkaxes([ax1,ax2,ax3], 'x');

%% Position
figure;
ax1 = subplot(3,1,1);
plot(sim_INS_Out.Values.x_R, 'Color', 'r');
hold on;
plot(ref_INS_Out.Values.x_R, 'Color', 'b');
grid on;
legend('x', 'x_r')

ax2 = subplot(3,1,2);
plot(sim_INS_Out.Values.y_R, 'Color', 'r');
hold on;
plot(ref_INS_Out.Values.y_R, 'Color', 'b');
grid on;
legend('y', 'y_r')

ax3 = subplot(3,1,3);
plot(sim_INS_Out.Values.h_R, 'Color', 'r');
hold on;
plot(ref_INS_Out.Values.h_R, 'Color', 'b');
grid on;
legend('h', 'h_r')

linkaxes([ax1,ax2,ax3], 'x');

%% FMS Output
figure;
ax1 = subplot(3,1,1);
plot(sim_FMS_Out.Values.u_cmd, 'Color', 'r');
hold on;
plot(ref_FMS_Out.Values.u_cmd, 'Color', 'b');
grid on;
legend('u\_cmd', 'u\_cmd_r')
ax2 = subplot(3,1,2);
plot(sim_FMS_Out.Values.v_cmd, 'Color', 'r');
hold on;
plot(ref_FMS_Out.Values.v_cmd, 'Color', 'b');
grid on;
legend('v\_cmd', 'v\_cmd_r')
ax3 = subplot(3,1,3);
plot(sim_FMS_Out.Values.w_cmd, 'Color', 'r');
hold on;
plot(ref_FMS_Out.Values.w_cmd, 'Color', 'b');
grid on;
legend('w\_cmd', 'w\_cmd_r')
linkaxes([ax1,ax2,ax3], 'x');

figure;
ax1 = subplot(3,1,1);
plot(sim_FMS_Out.Values.phi_cmd, 'Color', 'r');
hold on;
plot(ref_FMS_Out.Values.phi_cmd, 'Color', 'b');
grid on;
legend('\phi\_cmd', '\phi\_cmd_r')
ax2 = subplot(3,1,2);
plot(sim_FMS_Out.Values.theta_cmd, 'Color', 'r');
hold on;
plot(ref_FMS_Out.Values.theta_cmd, 'Color', 'b');
grid on;
legend('\theta\_cmd', '\theta\_cmd_r')
ax3 = subplot(3,1,3);
plot(sim_FMS_Out.Values.psi_rate_cmd, 'Color', 'r');
hold on;
plot(ref_FMS_Out.Values.psi_rate_cmd, 'Color', 'b');
grid on;
legend('\psi\_rate\_cmd', '\psi\_rate\_cmd_r')
linkaxes([ax1,ax2,ax3], 'x');

%% Control Output
figure;

ax1 = subplot(4,1,1);
plot(sim_Control_Out.Values.actuator_cmd.Time, sim_Control_Out.Values.actuator_cmd.Data(:,1), 'Color', 'r');
hold on;
plot(ref_Control_Out.Values.actuator_cmd.Time, ref_Control_Out.Values.actuator_cmd.Data(:,1), 'Color', 'b');
grid on;
legend('motor1', 'motor1_r')

ax2 = subplot(4,1,2);
plot(sim_Control_Out.Values.actuator_cmd.Time, sim_Control_Out.Values.actuator_cmd.Data(:,2), 'Color', 'r');
hold on;
plot(ref_Control_Out.Values.actuator_cmd.Time, ref_Control_Out.Values.actuator_cmd.Data(:,2), 'Color', 'b');
grid on;
legend('motor2', 'motor2_r')

ax3 = subplot(4,1,3);
plot(sim_Control_Out.Values.actuator_cmd.Time, sim_Control_Out.Values.actuator_cmd.Data(:,3), 'Color', 'r');
hold on;
plot(ref_Control_Out.Values.actuator_cmd.Time, ref_Control_Out.Values.actuator_cmd.Data(:,3), 'Color', 'b');
grid on;
legend('motor3', 'motor3_r')

ax4 = subplot(4,1,4);
plot(sim_Control_Out.Values.actuator_cmd.Time, sim_Control_Out.Values.actuator_cmd.Data(:,4), 'Color', 'r');
hold on;
plot(ref_Control_Out.Values.actuator_cmd.Time, ref_Control_Out.Values.actuator_cmd.Data(:,4), 'Color', 'b');
grid on;
legend('motor4', 'motor4_r')

linkaxes([ax1,ax2,ax3,ax4], 'x');


function h = thermometer(varargin)
% Create a Thermometer plot.
% THERMOMETER(DATA, MAX) - Create a thermometer
%    where DATA is a set of 1D data to plot in the thermometer.
%    Each entry in DATA adds onto the previous number, thus:
%    [ 3 4 5 ] will show the '3' between 0 and 3, and '4' between 3
%    and 7, and '5' between 7 and 12.
% THERMOMETER(DATA, MIN, MAX)
%    MIN and MAX represent the minimum and maximum thermometer
%    values.
%    Values in DATA add onto MIN. Thus, if MIN is 10, and DATA is
%    [ 3 4 ] then the '3' is shown between 10 and 13, and the '4'
%    between 13 and 17.
% THERMOMETER(..., { 'name1', 'name2' })
%    Area labels.
% THERMOMETER(..., { GOAL 'label' })
%    At a specific GOAL, add LABEL.
%    The GOAL value is absolute, and does not change based on MIN
%    or MAX.
%
% The thermometer code has been tweeked to look good printed from
% painters.  Using Zbuffer or OpenGL will make it looks strange.
%
% To adjust the tick labels, modify GCA's yticks.
%
% To make the zone labels alternate sides, uncomment the section
% that contains the word 'left'.
%
% Example:
% thermometer([ 3 4 ], 10, { 'tasks' 'bugs' }, { 8 'Almost There' })

% History:
%  Version 1.2: Update doc to better describe data.
%
%  Version 1.1:  Fixed problem using color-order for shading
%  elements so more than 7 entries can be used.
%
%
% Eric Ludlam
% Copyright (c) 2006 The MathWorks Inc.

  data = varargin{1};

  if any(data < 0)
      error('DATA Values for thermometer must be > 0.');
  end

  if nargin >= 3 && isnumeric(varargin{3})
    inputy = [ varargin{2} varargin{3} ];
    base = 4;
  else
    inputy = [ 0 varargin{2} ];
    base = 3;
  end

  names = {};
  goals = {};

  while nargin >= base

    nexta = varargin{base};

    if ~iscell(nexta)
      error('Arguments after MAX must be cell arrays');
    end

    if ischar(nexta{1})

      names = cell(size(data,1),1);
      names(1:length(varargin{base})) = varargin{base};

    elseif isnumeric(nexta{1})

      goals = nexta;

    else

      error('Unknown cell array format for thermometer.', nexta); %#ok

    end

    base = base + 1;
  end

  lw = 2;
  ax = newplot;

  set(ax,'color','white');
  total = inputy(2) - inputy(1);
  yl = [ inputy(1)-(total/15) inputy(2) ];
  set(ax,'ylim', yl)
  set(ax,'xlim', [ 0 1 ])
  set(ax,'xtick', [] )
  set(ax,'plotboxaspectratio',[ 1 20 1 ]);
  set(ax,'box','on');
  set(ax,'tickdir','out');
  set(ax,'ygrid','on');
  set(ax,'gridlinestyle','-');
  set(ax,'box','off');

  co = get(gca,'colororder');

  l = get(gca,'loose');
  l(1) = l(end);
  set(gca,'loose',l);

  % Draw a circle at bottom of thermometer

  rectangle('position',[-.5 inputy(1)-(total/15*1.5) 2 total/15*1.6 ],...
            'curvature',[ 1 1 ],...
            'facecolor',co(1,:),'edgecolor','k',...
            'clipping','off',...
            'linewidth',3);

  % Data Patchs

  % One number being plotted

  surface('xdata', [ 0 1 ] , 'ydata', [ yl(1) data(1)+inputy(1) ], ...
          'zdata', [ 1 1 ; 1 1 ],  ...
          'facecolor', co(1,:) ,'edgecolor','none');

  if ~isempty(names)
    yzonelabel_t(inputy(1), data(1)+inputy(1), names{1}, 'right');
  end

  bottom = data(1)+inputy(1);

  % A stacked data plot
  for i = 2:length(data)

    cidx = i-1;
    color = co(mod(cidx,size(co,1))+1,:);

    surface('xdata', [ 0 1 ] , 'ydata', [ bottom bottom+data(i) ], ...
            'zdata', [ 0 0 ; 0 0 ],  ...
            'facecolor', color ,'edgecolor','none');
    %if ~mod(i,2)
    %  lr = 'left';
    %else
    lr = 'right';
    %end

    if ~isempty(names) && length(names) >= i
      yzonelabel_t(bottom, bottom+data(i), names{i}, lr);
    end

    bottom = bottom+data(i);

  end

  while ~isempty(goals)

    pointlabel(goals{1}, goals{2}, false);

    goals = goals(3:end);

  end

    % Put a new box onto the axes.
  line('xdata',[0 0 1 1],'ydata', [ inputy(1) yl(2) yl(2) inputy(1) ],...
       'zdata',[1 1 1 1],...
       'color','k','linewidth',lw,'clipping','off');

  if nargout > 0
      h = ax;
  end

end

function pointlabel(yval, str, left)
% Create a fancy label at yval

  if left
    xdata = [ 0 -.7 ];
    txtpos = -.8;
    ha = 'right';
  else
    xdata = [ 1 1.7 ];
    txtpos = 1.8;
    ha = 'left';
  end
  ydata = [ yval yval ];

  line(xdata,ydata,'color','k','clipping','off');
  line(1,yval,'color','k',...
       'markersize',6,...
       'marker','diamond','markerfacecolor','k');
  line(0,yval,'color','k',...
       'markersize',6,...
       'marker','diamond','markerfacecolor','k');
  line([0 1],ydata,'color','k','linestyle',':');
  text(txtpos,yval,str,'horizontalalign',ha,...
       'verticalalign','middle');

end

function yzonelabel_t(start, fin, str, side)
% Create a small label thingy between START and END
% STR is an optional string to add to the label.  The text
% representing the total Y value encompassed is also included.
% SIDE is a string, either 'left', or 'right'.

  xl = xlim;
  xr = xl(2);
  xl = xl(1);
  factor = abs(xl-xr);

  switch side
   case 'left'
    in = xl-1.2*factor;
    out = xl-2*factor;
    xdata = [ in out out in ];
    txtpos = xl-2.1*factor;
    rot = 90;
   case 'right'
    in = xr + .5*factor;
    out = xr + .7*factor;
    xdata = [ in out out in ];
    txtpos = xr + .8*factor;
    rot = -90;
  end
  ydata = [ start start fin fin ];

  mid = (start + fin) / 2;

  numstr = num2str(fin-start);

  if isempty(str)
    newstr = numstr;
  else
    newstr = [ numstr char(10) str ];
  end

  l = line(xdata,ydata,'color','k');
  t = text(txtpos, mid, newstr,'rotation',rot,...
           'fontsize',10,...
           'horizontalalign','center',...
           'verticalalign','bottom');
  set([ l t ], 'xliminclude','off',...
               'clipping','off');

end


function opts = updateMovie(S, dt, p, opts, t, v, compGrid, plotGrid)
%UPDATEMOVIE   Update the movie when solving a PDE specified by a SPINOPSPHERE.

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

% Set-up:
nVars = S.numVars;
Clim = opts{1};
dataToPlot = opts{3};
ll = compGrid{1};
tt = compGrid{2};
N = 2*(size(ll, 1) - 1);
lll = plotGrid{1};
ttt = plotGrid{2};
Nplot = 2*(size(lll, 1) - 1);

for k = 1:nVars

    % Extract each variable:
    idx = (k-1)*N + 1;
    vv = dataToPlot(v(idx:idx+N-1,:));
    vv = [vv, vv(:,1)]; %#ok<*AGROW> add repeated values (periodic endpoints)
    vv = [vv; vv(1,:)];
    vv = vv([N/2+1:N 1], :); % extract values that correspond to theta in [0,pi]

    % Change axes if necessary:
    if ( nargout == 1 )
        minvnew = min(vv(:));
        maxvnew = max(vv(:));
        if ( maxvnew > Clim(2*(k-1) + 2) )
            vscalenew = max(abs(minvnew), maxvnew);
            Clim(2*(k-1) + 2) = maxvnew + .1*vscalenew;
        end
        if ( minvnew < Clim(2*(k-1) + 1) )
            vscalenew = max(abs(minvnew), maxvnew);
            Clim(2*(k-1) + 1) = minvnew - .1*vscalenew;
        end
    end

    % Interpolate each variable on a finer grid:
    if ( Nplot > N )
        vvv = interp2(ll, tt, vv, lll, ttt, 'spline');
    else
        vvv = vv;
    end

    % Update each variable:
    set(p{1,k}, 'cdata', vvv)
    set(p{1,k}.Parent, 'clim', [Clim(2*(k-1) + 1), Clim(2*(k-1) + 2)])

    % Update title:
    titleString = sprintf('n = m = %i (DoFs = %i), dt = %1.1e, t = %.4f', N, ...
        nVars*N^2, dt, t);
    set(p{2,k}, 'String', titleString)
    drawnow

end

% Update outputs:
opts{1} = Clim;

end


function [predictedY,predictedYD,YD,stats] = predictPhenotype_CVHMM (Yin,Fin,T,...
    options_HMM,options_prediction,varargin)
%
% Kernel ridge regression or nearest-neighbour estimation using
% a distance matrix using (stratified) LOO.
% The difference with predictPhenotype.m is that the HMM is run also within
% the cross-validation scheme (run on training data, reapplied on test), so it's slower.
%
% Since the HMM is purely unsupervised (does not use Yin), it is
% safe to run it just once, and have only the prediction within the
% cross-validation loop (whether this is actually OK depends on the specific application).
% For this, use computeDistMatrix() and predictPhenotype()
%
% INPUT
% Yin       (no. subjects by no. phenotypes) matrix of phenotypic values to predict,
%           where each element can be continuous or binary. If a multiclass variable
%           is to be predicted, then Yin should be encoded by a
%           (no. subjects by no. classes) matrix, with zeros or ones
%           indicator entries.
% Fin       (no. subjects by 1) cell with (i) files of subject data, or (ii) just the data
% options   Struct with the prediction options, with fields:
%   + alpha - for method='KRR', a vector of weights on the L2 penalty on the regression
%           By default: [0.0001 0.001 0.01 0.1 0.4 0.7 1.0 10 100]
%   + sigmafact - for method='KRR', a vector of parameters for the kernel; in particular,
%           this is the factor by which we will multiply a data-driven estimation
%           of the kernel parameter. By default: [1/5 1/3 1/2 1 2 3 5];
%   + CVscheme - vector of two elements: first is number of folds for model evaluation;
%             second is number of folds for the model selection phase (0 in both for LOO)
%   + CVfolds - prespecified CV folds for the outer loop
%   + biascorrect - whether we correct for bias in the estimation
%                   (Smith et al. 2019, NeuroImage)
%   + verbose -  display progress?
% cs        optional (no. subjects X no. subjects) dependency structure matrix with
%           specifying possible relations between subjects (e.g., family
%           structure), or a (no. subjects X 1) vector defining some
%           grouping, with (1...no.groups) or 0 for no group
% confounds     (no. subjects by  no. of confounds) matrix of features that
%               potentially influence the phenotypes and we wish to control for
%               (optional)
%
% OUTPUT
% predictedY    predicted response,in the original (non-decounfounded) space
% predictedYD    predicted response,in the decounfounded space
% YD    response,in the decounfounded space
% stats         structure, with fields
%   + pval - permutation-based p-value, if permutation is run;
%            otherwise, correlation-based p-value
%   + cod - coeficient of determination
%   + corr - correlation between predicted and observed Y
%   + baseline_corr - baseline correlation between predicted and observed Y for null model
%   + sse - sum of squared errors
%   + baseline_sse - baseline sum of squared errors
%   PLUS: All of the above +'_deconf' in the deconfounded space, if counfounds were specified
%
% Author: Diego Vidaurre, OHBA, University of Oxford

options = options_prediction;
[N,q] = size(Yin);

if ~iscell(Fin), error('Argument Fin must be a cell'); end

which_nan = false(N,1);
if q == 1
    which_nan = isnan(Yin);
    if any(which_nan)
        Yin = Yin(~which_nan);
        Fin = Fin(~which_nan,:);
        warning('NaN found on Yin, will remove...')
    end
    N = size(Yin,1);
end

if isempty(options), options = struct(); end
if nargin<3, options = struct(); end

if ~isfield(options,'alpha')
    alpha = [0.0001 0.001 0.01 0.1 0.4 0.7 1.0 10 100];
else
    alpha = options.alpha;
end
if ~isfield(options,'sigmafact')
    sigmafact = [1/5 1/3 1/2 1 2 3 5];
else
    sigmafact = options.sigmafact;
end
if ~isfield(options,'K')
    K = 1:min(50,round(0.5*N));
else
    K = options.K;
end

if ~isfield(options,'CVscheme'), CVscheme = [10 10];
else, CVscheme = options.CVscheme; end
if ~isfield(options,'CVfolds'), CVfolds = [];
else, CVfolds = options.CVfolds; end
% if ~isfield(options,'biascorrect'), biascorrect = 0;
% else, biascorrect = options.biascorrect; end
if ~isfield(options,'verbose'), verbose = 1;
else, verbose = options.verbose; end

% check correlation structure
allcs = [];
if (nargin>=6) && ~isempty(varargin{1})
    cs = varargin{1};
    if ~isempty(cs)
        is_cs_matrix = (size(cs,2) == size(cs,1));
        if size(cs,2)>1 % matrix format
            if any(which_nan)
                cs = cs(~which_nan,~which_nan);
            end
            [allcs(:,2),allcs(:,1)]=ind2sub([length(cs) length(cs)],find(cs>0));
        else
            if any(which_nan)
                cs = cs(~which_nan);
            end
            allcs = find(cs > 0);
        end
    end
else, cs = [];
end

% get confounds
if (nargin>=7) && ~isempty(varargin{2})
    confounds = varargin{2};
    confounds = confounds - repmat(mean(confounds),N,1);
    deconfounding = 1;
    if any(which_nan)
        confounds = confounds(~which_nan,:);
    end
else
    confounds = []; deconfounding = 0;
end

Ymean = zeros(N,q);
YD = zeros(N,q); % deconfounded signal
YmeanD = zeros(N,q); % mean in deconfounded space
predictedY = zeros(N,q);
if deconfounding, predictedYD = zeros(N,q); end

% create the inner CV structure - we can't to stratified because it's the
% same fold structure for all variables
if isempty(CVfolds)
    if CVscheme(1)==1
        folds = {1:N};
    elseif q == 1
        Yin_copy = Yin; Yin_copy(isnan(Yin)) = realmax;
        folds = cvfolds(Yin_copy,CVscheme(1),allcs);
    else  % no stratification
        folds = cvfolds(randn(size(Yin,1),1),CVscheme(1),allcs);
    end
else
    folds = CVfolds;
end

for ifold = 1:length(folds)

    if verbose, fprintf('CV iteration %d \n',ifold); end

    J = folds{ifold}; % test
    if isempty(J), continue; end
    if length(folds)==1
        ji = J;
    else
        ji = setdiff(1:N,J); % train
    end

    hmm = hmmmar(Fin(ji),T(ji),options_HMM);
    HMMs_dualregr = cell(N,1);

    % dual-estimation
    for j = 1:N
        if ischar(Fin{j})
            fsub = Fin{j};
            loadfile_sub;
        else
            X = Fin{j};
        end
        HMMs_dualregr{j} = hmmdual(X,T{j},hmm);
        HMMs_dualregr{j}.state = rmfield(HMMs_dualregr{j}.state,'prior');
    end

    disp('HMM done')

    Din = zeros(N,N);
    parfor n1 = 1:N-1
        din = zeros(1,N);
        for n2 = n1+1:N
            % FO is contained in TPC; TPC is contained in HMM
             din(n2) = (hmm_kl(HMMs_dualregr{n1},HMMs_dualregr{n2}) ...
                + hmm_kl(HMMs_dualregr{n2},HMMs_dualregr{n1}))/2;
        end
        Din(n1,:) = din;
    end; Din = Din' + Din;

    D = Din(ji,ji);
    Y = Yin(ji,:);
    D2 = Din(J,ji);

    % family structure for this fold
    Qallcs=[];
    if (~isempty(cs))
        if is_cs_matrix
            [Qallcs(:,2),Qallcs(:,1)] = ...
                ind2sub([length(cs(ji,ji)) length(cs(ji,ji))],find(cs(ji,ji)>0));
        else
            Qallcs = find(cs(ji) > 0);
        end
    end

    parfor ii = 1:q

        Dii = D; Yii = Y;

        ind = find(~isnan(Y(:,ii)));
        Yii = Yii(ind,ii);
        QDin = Dii(ind,ind);
        QN = length(ind);

        Qfolds = cvfolds(Yii,CVscheme(2),Qallcs); % we stratify

        % deconfounding business
        if deconfounding
            Cii = confounds(ji,:); Cii = Cii(ind,:);
            [betaY,interceptY,Yii] = deconfoundPhen(Yii,Cii);
        end

        % centering response
        my = mean(Yii);
        Yii = Yii - repmat(my,size(Yii,1),1);
        Ymean(J,ii) = my;
        QYin = Yii;

        Dev = Inf(length(alpha),length(sigmafact));

        for isigm = 1:length(sigmafact)

            sigmf = sigmafact(isigm);

            QpredictedY = Inf(QN,length(alpha));
            QYinCOMPARE = QYin;

            % Inner CV loop
            for Qifold = 1:length(Qfolds)

                QJ = Qfolds{Qifold}; Qji = setdiff(1:QN,QJ);
                QD = QDin(Qji,Qji);
                QY = QYin(Qji); Qmy = mean(QY); QY = QY-Qmy;
                Nji = length(Qji);
                QD2 = QDin(QJ,Qji);

                sigmabase = auto_sigma(QD);
                sigma = sigmf * sigmabase;

                K = gauss_kernel(QD,sigma);
                K2 = gauss_kernel(QD2,sigma);
                I = eye(Nji);
                ridg_pen_scale = mean(diag(K));

                for ialph = 1:length(alpha)
                    alph = alpha(ialph);
                    beta = (K + ridg_pen_scale * alph * I) \ QY;
                    QpredictedY(QJ,ialph) = K2 * beta + repmat(Qmy,length(QJ),1);
                end
            end

            Dev(:,isigm) = (sum(( QpredictedY - ...
                repmat(QYinCOMPARE,1,length(alpha))).^2) / QN)';

        end

        [~,m] = min(Dev(:)); % Pick the one with the lowest deviance
        [ialph,isigm] = ind2sub(size(Dev),m);
        sigmf = sigmafact(isigm);
        sigmabase = auto_sigma(D);
        sigma = sigmf * sigmabase;
        alph = alpha(ialph);

        Dii = D(ind,ind); D2ii = D2(:,ind);

        K = gauss_kernel(Dii,sigma);
        K2 = gauss_kernel(D2ii,sigma);
        Nji = length(ind);
        I = eye(Nji);

        ridg_pen_scale = mean(diag(K));
        beta = (K + ridg_pen_scale * alph * I) \ Yii;

        % predict the test fold
        predictedY(J,ii) = K2 * beta + my; % some may be NaN actually

        % predictedYD and YD in deconfounded space; Yin and predictedY are confounded
        predictedYD(J,ii) = predictedY(J,ii);
        YD(J,ii) = Yin(J,ii);
        YmeanD(J,ii) = Ymean(J,ii);
        if deconfounding % in order to later estimate prediction accuracy in deconfounded space
            [~,~,YD(J,ii)] = deconfoundPhen(YD(J,ii),confounds(J,:),betaY,interceptY);
            % original space
            predictedY(J,ii) = confoundPhen(predictedY(J,ii),confounds(J,:),betaY,interceptY);
            Ymean(J,ii) = confoundPhen(YmeanD(J,ii),confounds(J,:),betaY,interceptY);
        end

%         if biascorrect % we do this in the original space
%             Yhattrain = K * beta + my;
%             if deconfounding
%                 Yhattrain = confoundPhen(Yhattrain,confounds(ji,:),betaY,interceptY);
%                 Ytrain = [confoundPhen(QYin,confounds(ji,:),betaY,interceptY) ...
%                     ones(size(QYin,1),1)];
%             else
%                 Ytrain = [QYin ones(size(QYin,1),1)];
%             end
%             b = pinv(Ytrain) * Yhattrain;
%             predictedY(J,ii) = (predictedY(J,ii) - b(2)) / b(1);
%         end

        if rem(ii,100)==0, disp(['Variable ' num2str(ii) ]); end

    end

    disp(['Fold ' num2str(ifold) ])

end

stats = struct();
stats.sse = zeros(q,1);
stats.cod = zeros(q,1);
stats.corr = zeros(q,1);
stats.baseline_corr = zeros(q,1);
stats.pval = zeros(q,1);
if deconfounding
    stats.sse_deconf = zeros(q,1);
    stats.cod_deconf = zeros(q,1);
    stats.corr_deconf = zeros(q,1);
    stats.baseline_corr_deconf = zeros(q,1);
    stats.pval_deconf = zeros(q,1);
end

for ii = 1:q
    ind = find(~isnan(Yin(:,ii)));
    stats.sse(ii) = sum((Yin(ind,ii)-predictedY(ind,ii)).^2);
    nullsse = sum((Yin(ind,ii)-Ymean(ind,ii)).^2);
    stats.cod(ii) = 1 - stats.sse(ii) / nullsse;
    stats.corr(ii) = corr(Yin(ind,ii),predictedY(ind,ii));
    stats.baseline_corr(ii) = corr(Yin(ind,ii),Ymean(ind,ii));
    [~,pv] = corrcoef(Yin(ind,ii),predictedY(ind,ii)); % original space
    if corr(Yin(ind,ii),predictedY(ind,ii))<0, stats.pval(ii) = 1;
    else, stats.pval(ii) = pv(1,2);
    end
    if deconfounding
        stats.sse_deconf(ii) = sum((YD(ind,ii)-predictedYD(ind,ii)).^2);
        nullsse_deconf = sum((YD(ind,ii)-YmeanD(ind,ii)).^2);
        stats.cod_deconf(ii) = 1 - stats.sse_deconf(ii) / nullsse_deconf;
        stats.corr_deconf(ii) = corr(YD(ind,ii),predictedYD(ind,ii));
        stats.baseline_corr_deconf(ii) = corr(YD(ind,ii),YmeanD(ind,ii));
        [~,pv] = corrcoef(YD(ind,ii),predictedYD(ind,ii)); % original space
        if corr(YD(ind,ii),predictedYD(ind,ii))<0, stats.pval_deconf(ii) = 1;
        else, stats.pval_deconf(ii) = pv(1,2);
        end
    end
end

end

function K = gauss_kernel(D,sigma)
% Gaussian kernel
D = D.^2; % because distance is sqrt-ed
K = exp(-D/(2*sigma^2));
end

function sigma = auto_sigma (D)
% gets a data-driven estimation of the kernel parameter
D = D(triu(true(size(D,1)),1));
sigma = median(D);
end

function [betaY,my,Y] = deconfoundPhen(Y,confX,betaY,my)
if nargin<3, betaY = []; end
if isempty(betaY)
    my = mean(Y);
    Y = Y - repmat(my,size(Y,1),1);
    betaY = (confX' * confX + 0.00001 * eye(size(confX,2))) \ confX' * Y;
end
Y = Y - confX*betaY;
end

function Y = confoundPhen(Y,conf,betaY,my)
Y = Y + conf*betaY + repmat(my,size(Y,1),1);
end


function test_failed = test_libltfat_fft(varargin)
test_failed = 0;

fprintf(' ===============  %s ================ \n',upper(mfilename));

definput.flags.complexity={'double','single'};
[flags]=ltfatarghelper({},definput,varargin);
dataPtr = [flags.complexity, 'Ptr'];

[~,~,enuminfo]=libltfatprotofile;
phaseconv = enuminfo.ltfat_phaseconvention;

fftwflags = struct('FFTW_MEASURE',0,'FFTW_ESTIMATE',64,'FFTW_PATIENT',32,'FFTW_DESTROY_INPUT',1,...
    'FFTW_UNALIGNED',2,'FFTW_EXHAUSTIVE',8,'FFTW_PRESERVE_INPUT',16);

Larr  = [351 350   9   1];
Warr  = [  10       3   3   1];

for idx = 1:numel(Larr)
    L = Larr(idx);
    W = Warr(idx);

    f = randn(L,W,flags.complexity) + 1i*randn(L,W,flags.complexity);
    fin = complex2interleaved(f);
    fPtr = libpointer(dataPtr,fin);

    c = cast(randn(L,W)+1i*randn(L,W),flags.complexity);
    cout = complex2interleaved(c);
    coutPtr = libpointer(dataPtr,cout);

    truec = fft(f);

    funname = makelibraryname('fft',flags.complexity,0);
    status = calllib('libltfat',funname,fPtr,L,W,coutPtr);

    res = norm(truec - interleaved2complex(coutPtr.Value),'fro');
    [test_failed,fail]=ltfatdiditfail(res+status,test_failed);
    fprintf(['FFT   L:%3i, W:%3i, %s %s %s\n'],L,W,flags.complexity,ltfatstatusstring(status),fail);

    % With plan
    c = cast(randn(L,W)+1i*randn(L,W),flags.complexity);
    cout = complex2interleaved(c);
    coutPtr = libpointer(dataPtr,cout);

    plan = libpointer();
    funname = makelibraryname('fft_init',flags.complexity,0);
    statusInit = calllib('libltfat',funname,L,W,fPtr,coutPtr, fftwflags.FFTW_MEASURE, plan);

    funname = makelibraryname('fft_execute',flags.complexity,0);

    statusExecute = calllib('libltfat',funname,plan);

    res = norm(truec - interleaved2complex(coutPtr.Value),'fro');
    [test_failed,fail]=ltfatdiditfail(res+statusInit,test_failed);
    fprintf(['FFT L:%3i, W:%3i, %s %s %s\n'],L,W,flags.complexity,ltfatstatusstring(status),fail);

    c = cast(randn(L,W)+1i*randn(L,W),flags.complexity);
    cout = complex2interleaved(c);
    coutPtr = libpointer(dataPtr,cout);

    funname = makelibraryname('fft_execute_newarray',flags.complexity,0);
    statusExecute = calllib('libltfat',funname,plan,fPtr,coutPtr);

    res = norm(truec - interleaved2complex(coutPtr.Value),'fro');
    [test_failed,fail]=ltfatdiditfail(res+statusInit,test_failed);
    fprintf(['FFT L:%3i, W:%3i, %s %s %s\n'],L,W,flags.complexity,ltfatstatusstring(status),fail);

    funname = makelibraryname('fft_done',flags.complexity,0);
    statusDone = calllib('libltfat',funname,plan);

    %%%%%% Inplace
    c = f;
    cout = complex2interleaved(c);
    coutPtr = libpointer(dataPtr,cout);

    plan = libpointer();
    funname = makelibraryname('fft_init',flags.complexity,0);
    statusInit = calllib('libltfat',funname,L,W, coutPtr, coutPtr, fftwflags.FFTW_MEASURE, plan);

    funname = makelibraryname('fft_execute',flags.complexity,0);
    statusExecute = calllib('libltfat',funname,plan);

    res = norm(truec - interleaved2complex(coutPtr.Value),'fro');
    [test_failed,fail]=ltfatdiditfail(res+statusInit,test_failed);
    fprintf(['FFT L:%3i, W:%3i, %s %s %s\n'],L,W,flags.complexity,ltfatstatusstring(status),fail);

    c = f;
    cout = complex2interleaved(c);
    coutPtr = libpointer(dataPtr,cout);

    funname = makelibraryname('fft_execute_newarray',flags.complexity,0);
    statusExecute = calllib('libltfat',funname,plan,coutPtr,coutPtr);

    res = norm(truec - interleaved2complex(coutPtr.Value),'fro');
    [test_failed,fail]=ltfatdiditfail(res+statusInit,test_failed);
    fprintf(['FFT L:%3i, W:%3i, %s %s %s\n'],L,W,flags.complexity,ltfatstatusstring(status),fail);

    funname = makelibraryname('fft_done',flags.complexity,0);
    statusDone = calllib('libltfat',funname,plan);
end


function [net,opts,x] = invert_nn_dw_pre(net, ref, opts)
% INVERT  Invert a CNN representation
% Parse the input arguments to override the above defaults
% Update the number of iterations using the learning rate if required
if isinf(opts.maxNumIterations)
    opts.maxNumIterations = numel(opts.learningRate) ;
end

net.regu = opts.regu;
net.fmask = opts.fmask;
% The size of the image that we are trying to obtain
x0_size = cat(2,net.normalization.imageSize,opts.numRepeats);
%keyboard

% x0_sigma is computed using a separate dataset.
% This is a useful normalization that helps scale the different terms in the
% optimization.
load('x0_sigma.mat', 'x0_sigma');

% Replicate the feature into a block. This is used for multiple inversions in parallel.
y0 = repmat(ref, [1 1 1 opts.numRepeats]);

% initial inversion image of size x0_size
if  ~isempty(opts.init)
    x= opts.init;
else
    x = randn(x0_size, 'single') ;
    %x= imfilter(x,fspecial('gaussian',50,30));
    %x = ones(x0_size, 'single') ;
    %x = zeros(x0_size, 'single') ;
    x = x / norm(x(:)) * x0_sigma  ;
end
x_momentum = zeros(x0_size, 'single') ;

switch opts.task
    case {0,-1}
        % allow reconstructing a subset of the representation by setting
        % a suitable mask on the features y0
        sf = 1:size(y0,3) ;
        if opts.filterGroup == 1
            sf= vl_colsubset(sf, 0.5, 'beginning') ;
        elseif opts.filterGroup == 2 ;
            sf= vl_colsubset(sf, 0.5, 'ending') ;
        end
        nx = min(opts.neigh, size(y0,2)) ;
        ny = min(opts.neigh, size(y0,1)) ;
        sx = (0:nx-1) + ceil((size(y0,2)-nx+1)/2) ;
        sy = (0:ny-1) + ceil((size(y0,1)-ny+1)/2) ;
        mask = zeros(size(y0), 'single') ;
        mask(sy,sx,sf,:) = 1 ;
        y0_sigma = norm(squeeze(y0(find(mask(:))))) ;
    case {1,-2,2}
        %mask = opts.mask;
        mask = ref;
        y0_sigma = 1;
end

%% Tweak the network by adding a reconstruction loss at the end

% This is saved here just for printing our progress as optimization proceeds
if net.cnn_mode==0
    switch opts.objective
        case 'l2'
            % Add the l2 loss over the network
            ly.type = 'custom' ;
            ly.w = y0 ;
            ly.mask = mask ;
            ly.forward = @nndistance_forward ;
            ly.backward = @nndistance_backward ;
            net.layers{end+1} = ly ;
        case 'l1'
            % The L1 loss might want to use a dropout layer.
            % This is just a guess and hasn't been tried.
            ly.type = 'dropout' ;
            ly.rate = opts.dropout ;
            net.layers{end+1} = ly ;
            ly.type = 'custom' ;
            ly.w = y0 ;
            ly.mask = mask ;
            ly.forward = @nndistance1_forward ;
            ly.backward = @nndistance1_backward ;
            net.layers{end+1} = ly ;
        case 'inner'
            % The inner product loss may be suitable for some networks
            ly.type = 'custom' ;
            ly.w = - y0 .* mask ;
            ly.forward = @nninner_forward ;
            ly.backward = @nninner_backward ;
            net.layers{end+1} = ly ;
        case 'oneclass'
            % The inner product loss may be suitable for some networks
            % maxize the probability of one class
            ly.type = 'custom' ;
            ly.mask =  mask ;
            ly.forward = @oneclass_forward ;
            ly.backward = @oneclass_backward ;
            net.layers{end+1} = ly ;
        otherwise
            error('unknown opts.objective') ;
    end
end

% --------------------------------------------------------------------
function res_ = oneclass_forward(ly, res, res_)
% --------------------------------------------------------------------
% the value for that one class
res_.x = -sum(reshape(res.x.*oneclass(ly.mask,res.x),1,[]));
% --------------------------------------------------------------------
function res = oneclass_backward(ly, res, res_)
% --------------------------------------------------------------------
% positive or negative ?
res.dzdx = -oneclass(ly.mask,res.x);

function out=oneclass(mask,res)
out = reshape(mask,size(res));
%out = repmat(reshape(mask,[1,1,numel(mask)]),[size(res,1),size(res,2),1]);

% --------------------------------------------------------------------
function res_ = nndistance_forward(ly, res, res_)
% --------------------------------------------------------------------
res_.x = nndistance(res.x, ly.w, ly.mask) ;

% --------------------------------------------------------------------
function res = nndistance_backward(ly, res, res_)
% --------------------------------------------------------------------
res.dzdx = nndistance(res.x, ly.w, ly.mask, res_.dzdx) ;

% --------------------------------------------------------------------
function y = nndistance(x,w,mask,dzdy)
% --------------------------------------------------------------------
if nargin <= 3
    d = x - w ;
    y = sum(sum(sum(sum(d.*d.*mask)))) ;
else
    y = dzdy * 2 * (x - w) .* mask ;
end

% --------------------------------------------------------------------
function res_ = l1loss_forward(ly, res, res_)
% --------------------------------------------------------------------
res_.x = ly.w*sum(abs(res.x(:)));

% --------------------------------------------------------------------
function res = l1loss_backward(ly, res, res_)
% --------------------------------------------------------------------
res.dzdx = zeros(size(res.x), 'single');
res.dzdx(res.x > 0) = single(ly.w)*res_.dzdx;
res.dzdx(res.x < 0) = -single(ly.w)*res_.dzdx;
res.dzdx(res.x == 0) = single(0);

% --------------------------------------------------------------------
function res_ = nndistance1_forward(ly, res, res_)
% --------------------------------------------------------------------
res_.x = nndistance1(res.x, ly.w, ly.mask) ;

% --------------------------------------------------------------------
function res = nndistance1_backward(ly, res, res_)
% --------------------------------------------------------------------
res.dzdx = nndistance1(res.x, ly.w, ly.mask, res_.dzdx) ;

% --------------------------------------------------------------------
function y = nndistance1(x,w,mask,dzdy)
% --------------------------------------------------------------------
if nargin <= 3
    d = x - w ;
    y = sum(sum(sum(sum(abs(d).*mask)))) ;
else
    y = dzdy * sign(x - w) .* mask ;
end

% --------------------------------------------------------------------
function res_ = nninner_forward(ly, res, res_)
% --------------------------------------------------------------------
res_.x = nninner(res.x, ly.w) ;

% --------------------------------------------------------------------
function res = nninner_backward(ly, res, res_)
% --------------------------------------------------------------------
res.dzdx = nninner(res.x, ly.w, res_.dzdx) ;

% --------------------------------------------------------------------
function y = nninner(x,w,dzdy)
% --------------------------------------------------------------------
if nargin <= 2
    y = sum(sum(sum(sum(w.*x)))) ;
else
    y = dzdy * w ;
end


function [index, new_cutoff] = find_cutoff_index(cutoff, t2_vals)
%-----------------------------------------------------------------------------------
%
%  [index, new_cutoff] = find_cutoff_index(cutoff, t2_vals)
%
%  Function to find index corresponding to closest T2 value to cutoff in range
%
%	Ives Levesque, Feb 2007
%
%-----------------------------------------------------------------------------------

if cutoff > max(t2_vals) | cutoff < min(t2_vals)
   error('Cutoff value not in range.')
end

index = find(t2_vals==cutoff);

if isempty(index)
    temp_index = max(find(t2_vals<cutoff));
    if abs(t2_vals(temp_index+1)-cutoff) < abs(t2_vals(temp_index)-cutoff)
        index = temp_index + 1;
    else
        index = temp_index;
    end
end

new_cutoff = t2_vals(index);

%-----------------------------------------------------------------------------------


function train_id_net_vgg16(varargin)
% -------------------------------------------------------------------------
% Part 4.1: prepare the data
% -------------------------------------------------------------------------

imdb = load('./dataset/Flickr30k-prepareurl_data.mat');
imdb = imdb.imdb;
load('./dataset/Flickr30k-prepare/dense_feature_word2.1.mat');
imdb.charcnn = wordcnn;
%imdb.charmean = mean(imdb.charcnn(:,:,:,imdb.images.set==1),4);
% -------------------------------------------------------------------------
% Part 4.2: initialize a CNN architecture
% -------------------------------------------------------------------------
net = resnet52_new_hope_word2_pool_vgg19();
net.conserveMemory = true;
im_mean = imdb.rgbMean;
net.meta.normalization.averageImage = im_mean;
%net.meta.normalization.charmean = imdb.charmean;
% -------------------------------------------------------------------------
% Part 4.3: train and evaluate the CNN

% -------------------------------------------------------------------------
opts.train.averageImage = net.meta.normalization.averageImage;
opts.train.batchSize = 32;
opts.train.continue = true;
opts.train.gpus = 2;
opts.train.prefetch = false ;
opts.train.nesterovUpdate = true ;
opts.train.expDir = './data/vgg19_batch32_new_hope_word2.1_pool_shift_both_drop0.75';
opts.train.derOutputs = {'objective_img',1,'objective_txt',1} ;
%opts.train.gamma = 0.9;
opts.train.momentum = 0.9;
%opts.train.constraint = 100;
opts.train.learningRate = [0.1*ones(1,240)] ;
opts.train.weightDecay = 0.0001;
opts.train.numEpochs = numel(opts.train.learningRate) ;
[opts, ~] = vl_argparse(opts.train, varargin) ;
% Call training function in MatConvNet
[net,info] = cnn_train_dag(net, imdb, @getBatch,opts) ;

% --------------------------------------------------------------------
function inputs = getBatch(imdb,batch,opts)
% --------------------------------------------------------------------
%-- img data
im_url = imdb.images.data(batch) ;
im = vl_imreadjpeg(im_url,'Pack','Resize',[224,224],'Flip',...
    'CropLocation','random','CropSize',[0.8,1],...
    'Interpolation', 'bicubic','NumThreads',24,... %'Brightness', double(0.1*imdb.rgbCovariance),...
    'SubtractAverage',imdb.rgbMean,...
    'CropAnisotropy',[3/4,4/3]);
oim = im{1}; %bsxfun(@minus,im{1},opts.averageImage);
label_img =  imdb.images.label(batch);

%-- txt data
batchsize = numel(batch);
label_txt =  imdb.images.label(batch);
txt_branch = bsxfun(@times,(batch-1),5) + randi(5,batchsize,1);
txt = single(imdb.charcnn(:,txt_branch));
% code for faster construct one-hot vector (remove loop)   but seems not
% faster.... so I give up it.
%{
txt_v = txt(:);
txt_v(:,2) = reshape(repmat(0:(batchsize-1),32,1),1,[])';
txt_v(:,3) = reshape(repmat(0:31,1,batchsize),1,[])';
txt_v = txt_v(txt_v(:,1)>0,:);
txt_v = txt_v(:,1)+ bsxfun(@times,txt_v(:,2),642368)... %20074*32
    +bsxfun(@times,txt_v(:,3),20074);
txtinput = gpuArray(zeros(1,20074,32,batchsize,'single'));
txtinput(txt_v) = 1;
txtinput = permute(txtinput,[1,3,2,4]);
%}
txtinput = zeros(1,32,20074,batchsize,'single');
for i=1:batchsize
    len = numel(find(txt(:,i)>0));
    location = randi(33-len);
    for j=1:len
        v = txt(j,i);
        txtinput(1,location,v,i)=1;
        location = location+1;
    end
end
txtinput = gpuArray(txtinput);
%}
%--
inputs = {'x0',gpuArray(oim),'data2',txtinput,'label_img',label_img,'label_txt',label_txt};


classdef prtRvMultinomial < prtRv & prtRvMemebershipModel
    % prtRvMultinomial  Multinomial random variable
    %
    %   RV = prtRvMultinomial creates a prtRvMultinomial object
    %   with an unknown number of categories with unspecified probabilities
    %   These properties can be set manually or by using the MLE method.
    %
    %   prtRvMultinomial operates on count matrices. Therfore, the DRAW()
    %   method outputs a matrix that is N x nCategories and has a single 1
    %   in each row. To draw integer categories you can use the
    %   DRAWINTEGER() method.  Similarly, the MLE function takes a count
    %   matrix as an input. Type help prtRvMultinomial.mle for more
    %   information.
    %
    %   RV = prtRvMultinomial(PROPERTY1, VALUE1,...) creates a
    %   prtRvMultinomial object RV with properties as specified by
    %   PROPERTY/VALUE pairs.
    %
    %   A prtRvMultinomial object inherits all properties from the
    %   prtRv class. In addition, it has the following properties:
    %
    %   nCategories   - number of integers modeled by the RV
    %   probabilities - A 1 x nCategories vector of doubles less than 1
    %                   that sum to 1, representing the probability of
    %                   each of the integers
    %
    %  A prtRvMultinomial object inherits all methods from the prtRv
    %  class. The MLE  method can be used to set the parameters from data.
    %  In addition, it has the the following methods:
    %
    %   x = R.drawIntegers(N) - Draws N integers with the corresponding
    %                           probabilities
    %
    %  Example:
    %
    %  data = rand(100,5);                  % Uniformly random data
    %  X = bsxfun(@eq,data,max(data,[],2)); % Generate data that has a
    %                                       % single 1 in each row
    %
    %  RV = prtRvMultinomial;               % Generate a prtRvMultinomial
    %  RV = mle(RV,X);                      % Estimate the parameters
    %
    %  RV.plotPdf()                         % Plot the pdf (pmf)
    %
    %   See also: prtRv, prtRvMvn, prtRvGmm, prtRvVq, prtRvKde

    properties (SetAccess = private)
        name = 'Multinomial Random Variable';
        nameAbbreviation = 'RVMulti';
    end

    properties (SetAccess = protected)
        isSupervised = false;
        isCrossValidateValid = true;
    end

    properties (Dependent)
        probabilities
    end

    properties (Dependent = true)
        nCategories  % The number of categories
    end

    properties (Hidden = true, SetAccess='private', GetAccess='private')
        probabilitiesDepHelper
    end

    properties (Hidden = true, Dependent = true)
        nDimensions
    end

    properties (Hidden = true)
        approximatelyEqualThreshold = 1e-4;
        nDrawsPerObservationDraw = [];
    end

    methods
        % The Constructor
        function R = prtRvMultinomial(varargin)
            R = constructorInputParse(R,varargin{:});
        end

        function R = mle(R,X)
            % RV = RV.mle(X) computes the maximum likelihood estimate based
            % the data X. X should be nObservations x nDimensions. X must
            % be a count matrix, consisting of only zeros and ones. A one
            % in the ith column indicates that the sample in the jth row is
            % of class i.
            %

            %             if ~isequal(unique(X(:)),[0;1])
            %                 error('prtRvMultinomial:invalidData','Input matrix must contain only ones and zeros');
            %             end

            % Do we really want to or need to enforce this?
            % Can't we allow learning of the probability from multinomial
            % draws that had N greater than 1?
            % I don't see why not. - KDM 2013-03-01
            %if ~prtUtilApproxEqual(sum(X,2),1,1e-6);
            %    error('prtRvMultinomial:invalidData','Rows of input data must contain no more than one "1"');
            %end

            X = R.dataInputParse(X); % Basic error checking etc

            N_bar = sum(X,1);
            R.probabilities = N_bar./sum(N_bar(:));
        end

        function vals = pdf(R,X)
            % PDF Output the pdf of the random variable evaluated at the points specified
            %
            % pdf = RV.pdf(X) returns  the pdf of the prtRv
            % object evaluated at X. X must be an N x nDims matrix, where
            % N is the number of locations to evaluate the pdf, and nDims
            % is the same as the number of dimensions, nDimensions, of the
            % prtRv object RV.

            [isValid, reasonStr] = R.isValid;
            assert(isValid,'PDF cannot yet be evaluated. This RV is not yet valid %s.',reasonStr);

            X = R.dataInputParse(X); % Basic error checking etc
            assert(isnumeric(X) && ndims(X)==2,'X must be a 2D numeric array.');
            assert(size(X,2) == R.nCategories,'Incorrect number of categories for this RV object. This RV object is defined to have %d categories, but the input data has only %d columns. Remember that prtRvMultinomial operates on count matrices.', R.nCategories, size(X,2))

            vals = sum(bsxfun(@times,X,R.probabilities),2);
        end

        function vals = logPdf(R,X)
            % LOGPDF Output the log pdf of the random variable evaluated at the points specified
            %
            % logpdf = RV.logpdf(X) returns the logarithm of value of the
            % pdf of the prtRv object evaluated at X. X must be an N x
            % nDims matrix, where N is the number of locations to evaluate
            % the pdf, and nDims is the same as the number of dimensions,
            % nDimensions, of the prtRv object RV.

            [isValid, reasonStr] = R.isValid;
            assert(isValid,'LOGPDF cannot yet be evaluated. This RV is not yet valid %s.',reasonStr);

            vals = log(pdf(R,X));
        end

        function vals = draw(R,N)
            % DRAW  Draw random samples from the distribution described by the prtRv object
            %
            % VAL = RV.draw(N) generates N random samples drawn from the
            % distribution described by the prtRv object RV. VAL will be a
            % N x nDimensions vector, where nDimensions is the number of
            % dimensions of RV.

            assert(numel(N)==1 && all(N==floor(N)) && all(N > 0),'N must be a positive integer scalar.')

            [isValid, reasonStr] = R.isValid;
            assert(isValid,'DRAW cannot yet be evaluated. This RV is not yet valid %s.',reasonStr);

            if ~isempty(R.nDrawsPerObservationDraw)
                nObsToDraw = R.nDrawsPerObservationDraw;
                vals = zeros(N,R.nCategories);
                for iBag = 1:N
                    cVals = zeros(nObsToDraw,R.nCategories);
                    cVals(sub2ind([nObsToDraw, R.nCategories], (1:nObsToDraw)',drawIntegers(R,nObsToDraw))) = 1;
                    vals(iBag,:) = sum(cVals,1);
                end
            else
                vals = zeros(N,R.nCategories);
                vals(sub2ind([N, R.nCategories], (1:N)',drawIntegers(R,N))) = 1;
            end
        end

        function vals = drawIntegers(R,N)
            % DRAW  Draw random integer samples from the distribution described by the prtRv object
            %
            % VAL = RV.draw(N) generates N random integer samples drawn from the
            % distribution described by the prtRv object RV. VAL will be a
            % N x nDimensions vector, where nDimensions is the number of
            % dimensions of RV.

            [isValid, reasonStr] = R.isValid;
            assert(isValid,'DRAWINTEGERS cannot yet be evaluated. This RV is not yet valid %s.',reasonStr);

            assert(numel(N)==1 && N==floor(N) && N > 0,'N must be a positive integer scalar.')

            vals = prtRvUtilRandomSample(R.probabilities, N);
        end

        function varargout = plotPdf(R,varargin)
            %plotPdf Plot the pdf of the RV
            %
            % rv.plotPdf() plots the pdf of rv
            [isValid, reasonStr] = R.isValid;
            assert(isValid,'plotPdf cannot yet be evaluated. This RV is not yet valid %s.',reasonStr);

            h = bar(1:R.nCategories,R.probabilities,'k');
            ylim([0 1])
            xlim(R.plotLimits());

            varargout = {};
            if nargout
                varargout = {h};
            end
        end

        function plotCdf(R,varargin) %#ok<MANU>
            % plotCDF Not implemented for this prtRv
            error('prt:prtRvMultinomial','plotCdf is not implimented for this prtRv');
        end

    end

    % Set methods
    methods
        function R = set.probabilities(R,probs)
            assert(abs(sum(probs)-1) < R.approximatelyEqualThreshold,'Probability vector must must sum to 1.')

            R.probabilitiesDepHelper = probs(:)';
        end
        function R = set.nCategories(R,val) %#ok<MANU,INUSD>
            error('prt:prtRvMultinomial','nCategories is a dependent property that cannot be set by the user. To set the number of categories, set "probabilities" to be a vector of the desired length.');
        end
    end

    % Get methods
    methods
        function val = get.probabilities(R)
            val = R.probabilitiesDepHelper;
        end
        function val = get.nCategories(R)
            if ~isempty(R.probabilities)
                val = length(R.probabilities);
            else
                val = [];
            end
        end
        function val = get.nDimensions(R)
            val = R.nCategories;
        end
    end

    methods (Hidden = true)
        function [val, reasonStr] = isValid(R)
            if numel(R) > 1
                val = false(size(R));
                for iR = 1:numel(R)
                    [val(iR), reasonStr] = isValid(R(iR));
                end
                return
            end

            val = ~isempty(R.probabilities);

            if val
                reasonStr = '';
            else
                reasonStr = 'because probabilities has not been set';
            end
        end

        function val = plotLimits(R)
            [isValid, reasonStr] = R.isValid;
            if isValid
                val = [0.5 0.5+R.nCategories];
            else

                error('multinomial:plotLimits','Plotting limits can not yet be determined. This RV is not yet valid %s.',reasonStr)
            end
        end

        function val = isPlottable(R) %#ok
            val = true; % Always plottable
        end

        function R = weightedMle(R,X,weights)
            assert(numel(weights)==size(X,1),'The number of weights must mach the number of observations.');

            weights = weights(:);

            N_bar = sum(bsxfun(@times,X,weights),1);

            R.probabilities = N_bar./sum(N_bar(:));
        end
    end
end


function drawAcrobot(t,z,p)

clf; hold on;
length = p.l1+p.l2;
axis equal; axis(length*[-1,1,-1,1]);

[p1,p2] = acrobotKinematics(z,p);
pos = [[0;0],p1,p2];

plot(0,0,'ks','MarkerSize',25,'LineWidth',4)
plot(pos(1,:),pos(2,:),'Color',[0.1, 0.8, 0.1],'LineWidth',4)
plot(pos(1,:),pos(2,:),'k.','MarkerSize',50)

title(['Acrobot Animation,  t = ' num2str(t,2)])

drawnow; pause(0.01);

end


function varargout = sin(varargin)
%SIN   Sine of a CHEBFUN2.

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

[varargout{1:nargout}] = sin@separableApprox(varargin{:});

end


function output = F_multi_softmax(input_layer, TaskVocabSizes)
input = input_layer.a;
[D,T,N] = size(input);

if N>1; [mask, variableLength] = CheckTrajectoryLength(input); end

if length(TaskVocabSizes)==1    % this is for when you have the same task, e.g. skip gram
    if D>TaskVocabSizes
        input2 = reshape(input, TaskVocabSizes, D/TaskVocabSizes, T, N);
        fakeLayer.a = input2;
        output = F_softmax(fakeLayer);
        output = reshape(output, D, T, N);
    else        % this is normal softmax
        output = F_softmax(input_layer);
    end

else    % this is for when you have different tasks
    % to be implemented
end

if N>1 && variableLength; output = PadShortTrajectory(output, mask, -1e10); end

end


function f = sin( f )
%SIN   Sine of a CHEBFUN2.

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

% Empty check:
if ( isempty( f ) )
    return
end

f = compose( f, @sin );

end


function chebvar(varargin)
%CHEBVAR   Short-cut for constructing CHEBFUN variables.
%   CHEBVAR arg1 arg2 ...
%   is short-hand notation for creating symbolic variables
%          arg1 = chebfun('arg1');
%          arg2 = chebfun('arg2'); ...
%   The outputs are created in the current workspace.
%
%   CHEBVAR arg1 arg2 ... DOM constructs the CHEBFUN objects on the domain DOM,
%   i.e.,  arg1 = chebfun('arg1', DOM);
%          arg2 = chebfun('arg2', DOM); ...
%
%   In both cases, the CHEBFUN is created according to the currently stored
%   default preferences.
%
%   Example:
%     chebvar x
%     f = sin(x)
%
% See also CHEBFUN, CHEBFUNPREF.

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

% Trivial case:
if ( nargin == 0 )
    return
end
dom = [];

% Locate valid variable names:
isVar = cellfun(@isvarname, varargin);

% The last entry may be a domain:
if ( ~all(isVar) )
    dom = str2num(varargin{end}); %#ok<ST2NM>
    if ( ~isempty(dom) )
        varargin(end) = [];
        isVar(end) = [];
    end
end

% Check validity of variable name:
if ( ~all(isVar) )
    error('CHEBFUN:chebvar:badName', 'Not a valid variable name.');
end

% Acquire some preferences:
pref = chebfunpref();
if ( isempty(dom) )
    dom = pref.domain;
end

% Loop over each of the inputs:
for k = numel(varargin):-1:1
    op = str2op(varargin{k});
    f = chebfun(op, dom, pref);
    assignin('caller', varargin{k}, f);
end

end

function op = str2op(op)
% This is here as it's a clean function with no other variables hanging around
% in the scope.
depVar = symvar(op);
if ( numel(depVar) ~= 1 )
    error('CHEBFUN:chebvar:indepVars', ...
        'Incorrect number of independent variables in string input.');
end
op = eval(['@(' depVar{:} ')', op]);
end


% SLAMTB  An EKF-SLAM algorithm with simulator and graphics.
%
%   This script performs multi-robot, multi-sensor, multi-landmark 6DOF
%   EKF-SLAM with simulation and graphics capabilities.
%
%   Please read slamToolbox.pdf in the root directory thoroughly before
%   using this toolbox.
%
%   - Beginners should not modify this file, just edit USERDATA.M and enter
%   and/or modify the data you wish to simulate.
%
%   - More advanced users should be able to create new landmark models, new
%   initialization methods, and possibly extensions to multi-map SLAM. Good
%   luck!
%
%   - Expert users may want to add code for real-data experiments.
%
%   See also USERDATA, USERDATAPNT, USERDATALIN.
%
%   Also consult slamToolbox.pdf in the root directory.

%   Created and maintained by
%   Copyright 2008, 2009, 2010 Joan Sola @ LAAS-CNRS.
%   Copyright 2011, 2012, 2013 Joan Sola.
%   Programmers (for parts of the toolbox):
%   Copyright David Marquez and Jean-Marie Codol @ LAAS-CNRS
%   Copyright Teresa Vidal-Calleja @ ACFR.
%   See COPYING.TXT for full copyright license.

%% OK we start here

% clear workspace and declare globals
clear
global Map

%% I. Specify user-defined options - EDIT USER DATA FILE userData.m

userData;           % user-defined data. SCRIPT.
% userDataPnt;        % user-defined data for points. SCRIPT.
% userDataLin;        % user-defined data for lines. SCRIPT.

%% II. Initialize all data structures from user-defined data in userData.m
% SLAM data
[Rob,Sen,Raw,Lmk,Obs,Tim]     = createSlamStructures(...
    Robot,...
    Sensor,...      % all user data
    Time,...
    Opt);

% Simulation data
[SimRob,SimSen,SimLmk,SimOpt] = createSimStructures(...
    Robot,...
    Sensor,...      % all user data
    World,...
    SimOpt);

% Graphics handles
[MapFig,SenFig]               = createGraphicsStructures(...
    Rob, Sen, Lmk, Obs,...      % SLAM data
    SimRob, SimSen, SimLmk,...  % Simulator data
    FigOpt);                    % User-defined graphic options

%% III. Initialize data logging
% TODO: Create source and/or destination files and paths for data input and
% logs.
% TODO: do something here to collect data for post-processing or
% plotting. Think about collecting data in files using fopen, fwrite,
% etc., instead of creating large Matlab variables for data logging.

% Clear user data - not needed anymore
clear Robot Sensor World Time   % clear all user data

%% IV. Main loop
for currentFrame = Tim.firstFrame : Tim.lastFrame

    % 1. SIMULATION
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % Simulate robots
    for rob = [SimRob.rob]

        % Robot motion
        SimRob(rob) = simMotion(SimRob(rob),Tim);

        % Simulate sensor observations
        for sen = SimRob(rob).sensors

            % Observe simulated landmarks
            Raw(sen) = simObservation(SimRob(rob), SimSen(sen), SimLmk, SimOpt) ;

        end % end process sensors

    end % end process robots

    % 2. ESTIMATION
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % Process robots
    for rob = [Rob.rob]

        % Robot motion
        % NOTE: in a regular, non-simulated SLAM, this line is not here and
        % noise just comes from the real world. Here, the estimated robot
        % is noised so that the simulated trajectory can be made perfect
        % and act as a clear reference. The noise is additive to the
        % control input 'u'.
        Rob(rob).con.u = SimRob(rob).con.u + Rob(rob).con.uStd.*randn(size(Rob(rob).con.uStd));
        Rob(rob) = motion(Rob(rob),Tim);

        Map.t = Map.t + Tim.dt;

        % Process sensor observations
        for sen = Rob(rob).sensors

            % Observe knowm landmarks
            [Rob(rob),Sen(sen),Lmk,Obs(sen,:)] = correctKnownLmks( ...
                Rob(rob),   ...
                Sen(sen),   ...
                Raw(sen),   ...
                Lmk,        ...
                Obs(sen,:), ...
                Opt) ;

            % Initialize new landmarks
            ninits = Opt.init.nbrInits(1 + (currentFrame ~= Tim.firstFrame));
            for i = 1:ninits
                [Lmk,Obs(sen,:)] = initNewLmk(...
                    Rob(rob),   ...
                    Sen(sen),   ...
                    Raw(sen),   ...
                    Lmk,        ...
                    Obs(sen,:), ...
                    Opt) ;
            end

        end % end process sensors

    end % end process robots

    % 3. VISUALIZATION
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    if currentFrame == Tim.firstFrame ...
            || currentFrame == Tim.lastFrame ...
            || mod(currentFrame,FigOpt.rendPeriod) == 0

        % Figure of the Map:
        MapFig = drawMapFig(MapFig,  ...
            Rob, Sen, Lmk,  ...
            SimRob, SimSen, ...
            FigOpt);

        if FigOpt.createVideo
            makeVideoFrame(MapFig, ...
                sprintf('map-%04d.png',currentFrame), ...
                FigOpt, ExpOpt);
        end

        % Figures for all sensors
        for sen = [Sen.sen]
            SenFig(sen) = drawSenFig(SenFig(sen), ...
                Sen(sen), Raw(sen), Obs(sen,:), ...
                FigOpt);

            if FigOpt.createVideo
                makeVideoFrame(SenFig(sen), ...
                    sprintf('sen%02d-%04d.png', sen, currentFrame),...
                    FigOpt, ExpOpt);
            end

        end

        % Do draw all objects
        drawnow;
    end

    % 4. DATA LOGGING
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % TODO: do something here to collect data for post-processing or
    % plotting. Think about collecting data in files using fopen, fwrite,
    % etc., instead of creating large Matlab variables for data logging.

end

%% V. Post-processing
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Enter post-processing code here

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

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

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function eventLog = ma_executeNBodyCoast(coastEvent, initialState, eventNum, celBodyData)
%ma_executeNBodyCoast Summary of this function goes here
%   Detailed explanation goes here

	soiSkipIds = coastEvent.soiSkipIds;
    refBody = coastEvent.refBody;
    massLoss = coastEvent.massloss;
    forceModel = coastEvent.forceModel;
	maxPropTime = coastEvent.maxPropTime;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Get global coast ODE solver event functions
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    events = ma_getGlobalEvents(initialState, celBodyData);

	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Propagate through the n revs, if applicable
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    eventLog = [];
    revs = coastEvent.revs;
    if(revs > 0)
        rVect = initialState(2:4);
        vVect = initialState(5:7);

        bodyID = initialState(8);
        bodyInfo = getBodyInfoByNumber(bodyID, celBodyData);

        [sma, ecc, ~, ~, ~, tru] = getKeplerFromState(rVect, vVect, bodyInfo.gm);

        if(ecc < 1)
            revsInitstate = initialState;
            for(i=1:revs) %#ok<*NO4LP>
                period = computePeriod(sma, bodyInfo.gm);
                eventLogCoastRev1 = ma_executeCoast_nBody_goto_dt(period/2, revsInitstate, eventNum, forceModel, true, soiSkipIds, massLoss, maxPropTime, events, true, celBodyData);
                eventLogCoastRev2 = ma_executeCoast_nBody_goto_tru(tru, eventLogCoastRev1(end,:), eventNum, forceModel, true, soiSkipIds, refBody, massLoss, maxPropTime, events, celBodyData);

                eventLog = [eventLog;eventLogCoastRev1; eventLogCoastRev2]; %#ok<AGROW>
                revsInitstate = eventLogCoastRev2(end,:);
            end

            coastINIState = eventLogCoastRev2(end,:);
        else
            coastINIState = initialState;
            warnStr = ['Could not perform ',num2str(revs),' revolutions during coast, initial orbit is not elliptical.'];
            addToExecutionWarnings(warnStr, eventNum, bodyID, celBodyData);
        end
    else
        coastINIState = initialState;
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Do the coast according to its sub-type
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    type = coastEvent.coastType;
    value = coastEvent.coastToValue;
    switch type
        case 'goto_ut'
            eventLogCoast = ma_executeCoast_nBody_goto_ut(value, coastINIState, eventNum, forceModel, true, soiSkipIds, massLoss, maxPropTime, events, celBodyData);

        case 'goto_dt'
            eventLogCoast = ma_executeCoast_nBody_goto_dt(value, coastINIState, eventNum, forceModel, true, soiSkipIds, massLoss, maxPropTime, events, true, celBodyData);

        case 'goto_tru'
            eventLogCoast = ma_executeCoast_nBody_goto_tru(value, coastINIState, eventNum, forceModel, true, soiSkipIds, refBody, massLoss, maxPropTime, events, celBodyData);

        case 'goto_apo'
            eventLogCoast = ma_executeCoast_nBody_goto_tru(pi, coastINIState, eventNum, forceModel, true, soiSkipIds, refBody, massLoss, maxPropTime, events, celBodyData);

        case 'goto_peri'
            eventLogCoast = ma_executeCoast_nBody_goto_tru(0, coastINIState, eventNum, forceModel, true, soiSkipIds, refBody, massLoss, maxPropTime, events, celBodyData);

        case 'goto_asc_node'
            eventLogCoast = ma_executeCoast_nBody_goto_node('asc', coastINIState, eventNum, forceModel, true, soiSkipIds, refBody, massLoss, maxPropTime, events, celBodyData);

        case 'goto_desc_node'
            eventLogCoast = ma_executeCoast_nBody_goto_node('desc', coastINIState, eventNum, forceModel, true, soiSkipIds, refBody, massLoss, maxPropTime, events, celBodyData);

        case 'goto_soi_trans'
            eventLogCoast = ma_executeCoast_nBody_goto_soi_trans(coastINIState, eventNum, forceModel, soiSkipIds, massLoss, maxPropTime, events, celBodyData);

        otherwise
            error(['Did not recongize coast of type ', type]);
    end

    eventLog = [eventLog; eventLogCoast];
end


function [q, g, solutionQP] = thermoFlux2QNty(model,solution,param)
% Given a steady state thermodynamically feasible flux vector, v, such that
%
%     S*v = b
%  l <= v <= u
%
% Compute q and g = N'*y such that
%
%     N*diag(q)*g = b - B*w
%
% where the stoichiometric matrix and flux vector are split into internal
% and external components: S = [N B] and v = [z; w],

[nMet,nRxn]=size(model.S);

if ~exist('param','var')
    param=struct();
end

if ~isfield(model,'SConsistentRxnBool')  || ~isfield(model,'SConsistentMetBool')
    if ~isfield(param,'SConsistentMethod')
        param.SConsistentMethod = 'findSExRxnInd';
    end
    switch param.SConsistentMethod
        case 'findSExRxnInd'
            %finds the reactions in the model which export/import from the model
            %boundary i.e. mass unbalanced reactions
            %e.g. Exchange reactions
            %     Demand reactions
            %     Sink reactions
            model = findSExRxnInd(model,[],param.printLevel-1);
            model.SConsistentMetBool= model.SIntMetBool;
            model.SConsistentRxnBool= model.SIntRxnBool;
        case 'findStoichConsistentSubset'
            % Finds the subset of `S` that is stoichiometrically consistent using
            % an iterative cardinality optimisation approach
            [SConsistentMetBool, SConsistentRxnBool, ...
                SInConsistentMetBool, SInConsistentRxnBool, ...
                unknownSConsistencyMetBool, unknownSConsistencyRxnBool, model] ...
                = findStoichConsistentSubset(model);
    end
else
    if length(model.SConsistentRxnBool)~=nRxn
        error('Length of model.SConsistentRxnBool must equal the number of cols of model.S')
    end
    if length(model.SConsistentMetBool)~=nMet
        error('Length of model.SConsistentMetBool must equal the number of rows of model.S')
    end
end

if ~isfield(param,'bigNum')
    param.bigNum = 1;
end

[model,rankK,nnzK,timeTaken] = internalNullspace(model);

z = solution.v(model.SConsistentRxnBool);

%                       * .A - LHS matrix
%                       * .b - RHS vector
%                       * .F - positive semidefinite matrix for quadratic part of objective (see above)
%                       * .c - Objective coeff vector
%                       * .lb - Lower bound vector
%                       * .ub - Upper bound vector
%                       * .osense - Objective sense for the linear part (-1 max, +1 min)
%                       * .csense - Constraint senses, a string containing the constraint sense for
%                         each row in A ('E', equality, 'G' greater than, 'L' less than).
[n,m]=size(model.K);
QP.A = [model.K'*diag(z); ones(1,n)];
QP.b = [zeros(m,1);1];
QP.csense(1:m+1) ='E';
QP.lb = zeros(n,1);
QP.ub = param.bigNum*ones(n,1);
QP.c = zeros(n,1);
QP.F = speye(n);
QP.osense = 1;

solutionQP = solveCobraQP(QP);

if solutionQP.stat == 1
    q0 = 1./solutionQP.full;
    q = NaN*ones(nRxn,1);
    q(model.SConsistentRxnBool)=q0;

    g0 = diag(solutionQP.full)*z;
    %N = model.S(:,model.SConsistentRxnBool);
    %y = N'\g0;
    g = NaN*ones(nRxn,1);
    g(model.SConsistentRxnBool)=g0;

    %dg0 = N'*y - g0;
    %dg = NaN*ones(nRxn,1);
    %dg(model.SConsistentRxnBool)=dg0;
else
    solutionQP
    error('thermoFlux2QNty: solveCobraQP did not solve')
end
end


function [D] = spm_eeg_inv_Mesh2Voxels(varargin)
% Convert a mesh representation of M/EEG power into a smoothed image
% FORMAT [D] = spm_eeg_inv_Mesh2Voxels(D,[val])
% Input:
% D        - MEEG object or filename of M/EEG mat-file (optional)
%
%     D.inv{val}.contrast.display:   display image at the end {true, [false]}
%     D.inv{val}.contrast.space:     native [0] or MNI {1} output space
%     D.inv{val}.contrast.format:    output file format {['image'], 'mesh'}
%     D.inv{val}.contrast.smoothing: # iterations for cortical smoothing
%
% Output:
% D        - MEEG object containing the new image filenames in fields:
%
%     D.inv{val}.contrast.fname
%__________________________________________________________________________
%
% Non-linear interpolation of a Mesh contrast into MNI Voxel space
% This routine is used to produce a 3D image canonical sMRI
% space (in voxel coordinates) from a cortical mesh (3D surface).
% This yields a NIfTI image of the summary statistics of the cortical
% activity for the effect of interest. This image can then enter the
% classical SPM routines for statistical testing.
% The [non-negative] mean square contrast is smoothed both on the mesh
% (using a graph Laplacian) and then in voxel-space using a conventional
% Gaussian filter.
%__________________________________________________________________________
% Copyright (C) 2007-2017 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_eeg_inv_Mesh2Voxels.m 7094 2017-06-06 11:14:10Z guillaume $

SVNrev = '$Rev: 7094 $';

%-Startup
%--------------------------------------------------------------------------
spm('FnBanner', mfilename, SVNrev);

%-Parse input arguments
%--------------------------------------------------------------------------
[D,val]      = spm_eeg_inv_check(varargin{:});

%-Get options
%--------------------------------------------------------------------------
try, space   = D.inv{val}.contrast.space;     catch, space  = 1; end
try, fmt     = D.inv{val}.contrast.format;    catch, fmt    = 'image'; end
try, smooth  = D.inv{val}.contrast.smoothing; catch, smooth = 8; end
try, Disp    = D.inv{val}.contrast.display;   catch, Disp   = 0; end

%-Time and Frequency windows of interest
%--------------------------------------------------------------------------
woi          = D.inv{val}.contrast.woi;
foi          = D.inv{val}.contrast.fboi;
Nw           = size(woi,1);

%-Get output image field of view and resolution
%--------------------------------------------------------------------------
if space
    sMRIfile = fullfile(spm('dir'),'canonical','avg152T2.nii');
else
    sMRIfile = D.inv{val}.mesh.sMRI;
end
Vin          = spm_vol(sMRIfile);

%-Get surface mesh
%--------------------------------------------------------------------------
if space
    m        = export(gifti(D.inv{val}.mesh.tess_mni),'patch');
else
    m        = export(gifti(D.inv{val}.mesh.tess_ctx),'patch');
end
nd           = D.inv{val}.inverse.Nd;

%-Accumulate mean of log-contrasts (over trials)
%==========================================================================
GL      = spm_mesh_smooth(m);

GW      = D.inv{val}.contrast.GW;

bytrial = iscell(GW{1});

if bytrial
    Ne  = cellfun(@numel,GW);
else
    Ne  = ones(1, numel(GW));
end

Nj      = numel(GW)/Nw;

k  = 1;
iw = [];
ie = [];
for c = 1:length(GW)
    if bytrial
        cGW = GW{c};
    else
        cGW = GW(c);
    end

    for t = 1:Ne(c)
        %-Smooth on the cortical surface
        %------------------------------------------------------------------
        ssq{k} = full(sparse(D.inv{val}.inverse.Is,1,cGW{t},nd,1));
        ssq{k} = spm_mesh_smooth(GL,ssq{k},smooth);

        %-Compute (truncated) moment
        %------------------------------------------------------------------
        lss        = log(ssq{k} + eps);
        i          = lss > (max(lss) - log(32));
        meanlss(k) = mean(lss(i));

        iw(k) = c;
        ie(k) = t;

        k = k + 1;
    end
end

scale = exp(mean(meanlss));

%-Save as meshes
%==========================================================================
if strcmpi(fmt,'mesh')
    [pth,name] = fileparts(D.fname);
    tag = cell(1,Nw);
    for i = 1:Nw
        tag{i} = ['t' sprintf('%g_', woi(i,:)) 'f' sprintf('%d_', foi)];
    end

    %-Save mesh topology
    %----------------------------------------------------------------------
    g = gifti(m);
    save(g,fullfile(D.path,[name '.surf.gii']));

    %-Save mesh data
    %----------------------------------------------------------------------
    spm_progress_bar('Init',numel(ssq),'Exporting as meshes','');
    fprintf('%-40s: %30s','Export as meshes','...please wait');         %-#
    for c = 1:numel(ssq)

        fprintf('%s%30s',repmat(sprintf('\b'),1,30),...
            sprintf('...mesh %d/%d',c,numel(ssq)));                     %-#

        %-Filename
        %------------------------------------------------------------------
        con       = mod(iw(c) - 1, Nj) + 1;
        str       = tag{ceil(iw(c)/Nj)};
        if bytrial, bt = sprintf('_%.0f',ie(c)); else bt = ''; end
        fname     = fullfile(D.path,...
            sprintf('%s_%.0f_%s%.0f%s.gii', name, val, str, con, bt));

        %-Normalise
        %------------------------------------------------------------------
        Contrast = ssq{c} / scale;
        %Contrast = Contrast.*(Contrast > exp(-8));

        %-Write mesh
        %------------------------------------------------------------------
        g = gifti;
        g.cdata = Contrast;
        g.private.metadata(1).name = 'SurfaceID';
        g.private.metadata(1).value = [name '.surf.gii'];
        save(g, fname, 'ExternalFileBinary');

        %-Store filename
        %------------------------------------------------------------------
        D.inv{val}.contrast.fname{c} = fname;

        spm_progress_bar('Set', c);

    end

    spm_progress_bar('Clear');
    fprintf('%s%30s\n',repmat(sprintf('\b'),1,30),'...done');           %-#

    return;
end

%-Normalise and embed in 3D-space
%==========================================================================
fprintf('%-40s: %30s','Writing images','...please wait');               %-#

[pth,name] = fileparts(D.fname);
tag = cell(1,Nw);
for i = 1:Nw
    tag{i} = ['t' sprintf('%d_', woi(i,:)) 'f' sprintf('%d_', foi)];
end

spm_progress_bar('Init',numel(ssq),'Interpolating images','');

con       = mod(iw - 1, Nj) + 1;
win       = ceil(iw/Nj);

ucon      = unique(con);
uwin      = unique(win);

n = 0;
for c = 1:length(ucon)
    for w = 1:numel(uwin)

        ind = find((con == ucon(c)) & (win == uwin(w)));
        str = tag{uwin(w)};

        fname     = fullfile(D.path,...
            sprintf('%s_%.0f_%s%.0f.nii', name, val, str, ucon(c)));

        %-Initialise image
        %------------------------------------------------------------------
        N      = nifti;
        N.dat  = file_array(fname, [Vin.dim, length(ind)], 'FLOAT32-LE');
        N.mat  = Vin.mat;
        N.mat0 = Vin.mat;
        create(N);

        for i = 1:length(ind)

            n = n+1;

            fprintf('%s%30s',repmat(sprintf('\b'),1,30),...
                sprintf('...image %d/%d',n,numel(ssq)));                %-#

            %-Normalise
            %--------------------------------------------------------------
            Contrast = ssq{ind(i)} / scale;

            %-Interpolate those values into voxels
            %--------------------------------------------------------------
            RECimage = spm_mesh_to_grid(m, Vin, Contrast);

            %-3D smoothing and thresholding
            %--------------------------------------------------------------
            spm_smooth(RECimage, RECimage, 1);
            RECimage = RECimage.*(RECimage > exp(-8));

            %-Write (smoothed and scaled) image
            %--------------------------------------------------------------
            N.dat(:, :, :, i) = RECimage;

            %-Store filename
            %--------------------------------------------------------------
            D.inv{val}.contrast.fname{n} = fname;

            spm_progress_bar('Set', n);

        end
    end
end

spm_progress_bar('Clear');
fprintf('%s%30s\n',repmat(sprintf('\b'),1,30),'...done');               %-#

%-Display
%==========================================================================
if Disp && ~spm('CmdLine'), spm_eeg_inv_image_display(D); end


function out = Rmdm(A, B)

for i = 1:size(A, 3)
    for j = 1:size(B, 3)
        res(i, j) = Rdis(A(:, :, i), B(:, :, j));
    end
    out(i) = find(res(i, :) == min(res(i, :)));
end


function write_surf(fname, vert, face)

% write_surf - FreeSurfer I/O function to write a surface file
%
% write_surf(fname, vert, face)
%
% writes a surface triangulation into a binary file
% fname - name of file to write
% vert  - Nx3 matrix of vertex coordinates
% face  - Mx3 matrix of face triangulation indices
%
% The face matrix here must be matlab compatible
% (no zero indices).  It is converted to FreeSurfer
% indices that start at zero.
%
% See also freesurfer_read_surf, freesurfer_write_curv, freesurfer_write_wfile

if(nargin ~= 3)
  fprintf('USAGE: freesurfer_write_surf(fname, vert, face)\n');
  return;
end

if size(vert,2) ~= 3,
    error('vert must be Nx3 matrix');
end

if size(face,2) ~= 3,
    error('face must be Mx3 matrix');
end

fprintf('...subtracting 1 from face indices for FreeSurfer compatibility.\n');
face = face - 1;

% open it as a big-endian file
fid = fopen(fname, 'wb', 'b') ;

TRIANGLE_FILE_MAGIC_NUMBER = 16777214 ;
fwrite3(fid, TRIANGLE_FILE_MAGIC_NUMBER);

vnum = size(vert,1) ;  % number of vertices
fnum = size(face,1) ;  % number of faces

% Ouput a couple of text lines with creation date
fprintf(fid,'created by %s on %s\n\n',getenv('USER'),datestr(now)); % creation date

fwrite(fid, vnum,'int32');
fwrite(fid, fnum,'int32');

% reshape vert into column array and write
vert = reshape(vert',size(vert,1)*size(vert,2),1);
fwrite(fid, vert,'float32');

% reshape face into column array and write
face = reshape(face',size(face,1)*size(face,2),1);
fwrite(fid, face,'int32');

fclose(fid) ;

return


% datasetCandi = {'siftsmall'};
datasetCandi = {'sift','gist'};
% datasetCandi = {'sift'};
% datasetCandi = {'gist'};
% datasetCandi = {'imagenet'};

% methodCandi = {'LSH','ITQ'};

% methodCandi = {'USPLH'};
% methodCandi = {'CPH'};
% methodCandi = {'SH'};
% methodCandi = {'ITQ'};
% methodCandi = {'IsoH' 'SpH'};
% methodCandi = {'LSH'};

% methodCandi = {'DSH'};
% methodCandi = {'BRE'};
% methodCandi = {'SpH'};

% methodCandi = {'IsoH1'};

methodCandi = {'LSH','SpH','BRE','USPLH','ITQ','IsoH','SH'};
% methodCandi = {'KLSH','AGH1','AGH2','CH'};

codelengthCandi = [32, 64, 128, 256, 512, 1024];

for d=1:length(datasetCandi)
    dataset = datasetCandi{d};

    for m=1:length(methodCandi)
        method = methodCandi{m};

        for c=1:length(codelengthCandi)

            codelength = codelengthCandi(c);

            ResultFile = ['../',dataset,'/hashingCodeLong.',num2str(codelength),'/',method,'table',upper(dataset),'32b_1'];
            bFound = checkFILEmkDIR(ResultFile);
            if(bFound)
                error('Result file exists!');
            end

            disp('==============================');
            disp([method,' ',num2str(codelength),'bit ',dataset]);
            disp('==============================');

            trainset = double(fvecs_read (['../',dataset,'/',dataset,'_base.fvecs']));
            testset = fvecs_read (['../',dataset,'/',dataset,'_query.fvecs']);
            trainset = trainset';
            testset = testset';

            trainStr = ['[model, trainB ,train_elapse] = ',method,'_learn(trainset, codelength);'];
            testStr = ['[testB,test_elapse] = ',method,'_compress(testset, model);'];
            eval(trainStr);
            eval(testStr);

            ntrain=size(trainB,1);
            ntest=size(testB,1);

            realcodelength = size(trainB,2);
            disp([method,' ',num2str(realcodelength),'bit learned. ',dataset]);

            nFile = fix(realcodelength/32);
            nRemain = mod(realcodelength,32);

            disp('==============================');
            disp(['Total training time: ',num2str(train_elapse)]);
            disp(['Total testing time: ',num2str(test_elapse)]);

            %continue;

            for j =1:nFile
                ResultFile = ['../',dataset,'/hashingCodeLong.',num2str(codelength),'/',method,'table',upper(dataset),'32b_',num2str(j)];

                fid = fopen(ResultFile,'w');
                fwrite(fid, 1, 'uint32');
                fwrite(fid, 32, 'uint32');
                fwrite(fid, ntrain, 'uint32');
                for i = 1 : ntrain
                    tmpV = uint32(0);
                    for k = 32*(j-1)+1:32*j
                        tmpV = bitshift(tmpV,1);
                        tmpV = tmpV + uint32(trainB(i,k));
                    end
                    fwrite(fid, tmpV, 'uint32');
                end
                fclose(fid);

                ResultFile = ['../',dataset,'/hashingCodeLong.',num2str(codelength),'/',method,'query',upper(dataset),'32b_',num2str(j)];
                fid = fopen(ResultFile,'w');
                fwrite(fid, 1, 'uint32');
                fwrite(fid, 32, 'uint32');
                fwrite(fid, ntest, 'uint32');
                for i = 1 : ntest
                    tmpV = uint32(0);
                    for k = 32*(j-1)+1:32*j
                        tmpV = bitshift(tmpV,1);
                        tmpV = tmpV + uint32(testB(i,k));
                    end
                    fwrite(fid, tmpV, 'uint32');
                end
                fclose(fid);
            end

            if nRemain > 0
                ResultFile = ['../',dataset,'/hashingCodeLong.',num2str(codelength),'/',method,'table',upper(dataset),'32b_',num2str(nFile+1)];

                fid = fopen(ResultFile,'w');
                fwrite(fid, 1, 'uint32');
                fwrite(fid, 32, 'uint32');
                fwrite(fid, ntrain, 'uint32');
                for i = 1 : ntrain
                    tmpV = uint32(0);
                    for k = 32*nFile+1:size(trainB,2)
                        tmpV = bitshift(tmpV,1);
                        tmpV = tmpV + uint32(trainB(i,k));
                    end
                    fwrite(fid, tmpV, 'uint32');
                end
                fclose(fid);

                ResultFile = ['../',dataset,'/hashingCodeLong.',num2str(codelength),'/',method,'query',upper(dataset),'32b_',num2str(nFile+1)];
                fid = fopen(ResultFile,'w');
                fwrite(fid, 1, 'uint32');
                fwrite(fid, 32, 'uint32');
                fwrite(fid, ntest, 'uint32');
                for i = 1 : ntest
                    tmpV = uint32(0);
                    for k = 32*nFile+1:size(testB,2)
                        tmpV = bitshift(tmpV,1);
                        tmpV = tmpV + uint32(testB(i,k));
                    end
                    fwrite(fid, tmpV, 'uint32');
                end
                fclose(fid);

            end

            disp('binary file saved!');
        end

    end
end


% concatenate cell array of 3D tensors
% assume that the cells are MxNxP_i tensors where M and N are shared by all
% cells, while P_i can be different.
% Generate a 3D tensor of MxNxsum(P_i).
%
function output = cell2mat_tensor3D(data, padnumber)
if nargin<2
    padnumber = 0;
end

precision = 'single';
if ~isempty(data) && ~isempty(data{1})
    precision = class(data{1}(1));
end

nCell = length(data);
for i=1:nCell
    [M(i),N(i),P(i)] = size(data{i});
    if M(i)*N(i)==0
        P(i)=0;
    end
end

if sum(abs(N-N(1)))==0  % if all cells have the same number of columns
    N = N(1);
else        % if some cells have less columns than others, zero pad them
    Nmax = max(N);
    for j=1:nCell
        data{j}(:,N(j)+1:Nmax,:) = padnumber;
    end
    N = max(N);
end

if sum(abs(P-1))==0     % all are matrices, not tensors
    output = cell2mat(data);
    output = reshape(output, max(M),N,nCell);
else
    output = zeros(max(M),N, sum(P), precision);
    for j=1:nCell
        idx1 = sum(P(1:j-1))+1;
        idx2 = sum(P(1:j));
        if idx1>idx2; continue; end
        [d1,d2,d3] = size(data{j});
        [n1,n2,n3] = size(output(:,:, idx1:idx2 ));

        if d1~=n1 || d2~=n2 || d3~=n3 %M(j)~=max(M) || idx2-idx1+1 ~= P(j)
            msg = sprintf('Warning: data{j} has a dimension of %d - %d - %d, while output(:,:,idx1:idx2) has a dimension of %d - %d - %d, %s\n', d1,d2,d3,n1,n2,n3, datestr(now));
            my_append('cell2mat_tensor3D_log.txt', msg);
            continue;
        end
        output(:,:, idx1:idx2 ) = data{j};
    end
end

end


%% Copyright (C) 2014-2016, 2022 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy is free software; you can redistribute it and/or modify
%% it under the terms of the GNU General Public License as published
%% by the Free Software Foundation; either version 3 of the License,
%% or (at your option) any later version.
%%
%% This software is distributed in the hope that it will be useful,
%% but WITHOUT ANY WARRANTY; without even the implied warranty
%% of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See
%% the GNU General Public License for more details.
%%
%% You should have received a copy of the GNU General Public
%% License along with this software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym atand (@var{x})
%% Symbolic inverse tan function with output in degrees.
%%
%% Example:
%% @example
%% @group
%% atand (sqrt (sym (3)))
%%   @result{} (sym) 60
%%
%% syms x
%% y = atand (x)
%%   @result{} y = (sym)
%%       180⋅atan(x)
%%       ───────────
%%            π
%% @end group
%% @end example
%%
%% @seealso{@@sym/tand, @@sym/atan}
%% @end defmethod

function y = atand(x)
  if (nargin ~= 1)
    print_usage ();
  end
  y = elementwise_op ('lambda a: deg(atan(a))', x);
end

%!error atand (sym(1), 2)
%!assert (isequaln (atand (sym(nan)), sym(nan)))

%!test
%! f1 = atand (sym(1)/2);
%! f2 = atand (1/2);
%! assert (double (f1), f2, -eps)

%!test
%! D = [1 2; 3 4]/4;
%! A = sym([1 2; 3 4])/4;
%! f1 = atand (A);
%! f2 = atand (D);
%! assert (double (f1), f2, -eps)