%% brute_force_tune
% Code to test the performance of various tuning parameters
% Works sorta like RANSAC I guess?
% Adam Werries 2016, see Apache 2.0 license.

k_max = 50;
% Specify ranges
accel_noise_PSD = logspace(-8, 5, 100);
gyro_noise_PSD = logspace(-8, 5, 100);
% Repeat arrays
accel_noise_PSD = repmat(accel_noise_PSD, [1 k_max]);
gyro_noise_PSD = repmat(gyro_noise_PSD, [1 k_max]);
% Generate random selections of each vector
num_items = length(accel_noise_PSD);
accel_i = randperm(num_items);
gyro_i = randperm(num_items);

rms_error_filter = Inf*ones(1,num_items);
max_error_filter = Inf*ones(1,num_items);
parfor i = 1:num_items
    fprintf('Iteration: %d, ANoise: %08.5e, GNoise: %08.5e\n', i, accel_noise_PSD(accel_i(i)), gyro_noise_PSD(gyro_i(i)));
    temp_conf = LC_KF_config;
    temp_conf.accel_noise_PSD = (accel_noise_PSD(accel_i(i)) * mug_to_mps2)^2;
    temp_conf.gyro_noise_PSD = (gyro_noise_PSD(gyro_i(i)) * deg_to_rad / 60)^2;
    [out_profile,out_IMU_bias_est,out_KF_SD] = Loosely_coupled_INS_GNSS(init_cond, filter_time, epoch, lla, gps, imu, temp_conf, est_IMU_bias);
    xyz = out_profile(:,2:4);
    if ~any(any(isnan(xyz))) && ~any(any(isinf(xyz)))
        llh = ecef2lla(xyz);
        [x,y] = deg2utm(llh(:,1),llh(:,2));
        x = x-min_x;
        y = y-min_y;

        distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2).^0.5;
        rms_error_filter(i) = rms(distance);
        max_error_filter(i) = max(distance);
    end
end

[minmax, i] = min(max_error_filter);
fprintf('\nBest max: %08.4f, rms is %08.4f\n', minmax, rms_error_filter(i));
fprintf('Best iteration for max: %d, ANoise: %08.5e, GNoise: %08.5e\n', i, accel_noise_PSD(accel_i(i)), gyro_noise_PSD(gyro_i(i)));
[minrms, i] = min(rms_error_filter);
fprintf('Best rms: %08.4f, max is %08.4f\n', minrms, max_error_filter(i));
fprintf('Best iteration for rms: %d, ANoise: %08.5e, GNoise: %08.5e\n', i, accel_noise_PSD(accel_i(i)), gyro_noise_PSD(gyro_i(i)));
[minrms, i] = min((rms_error_filter+max_error_filter)/2);
fprintf('Best average of RMS and max: %08.4f, rms is  %08.4f, max is %08.4f\n', minrms, rms_error_filter(i), max_error_filter(i));
fprintf('Best iteration for rms: %d, ANoise: %08.5e, GNoise: %08.5e\n', i, accel_noise_PSD(accel_i(i)), gyro_noise_PSD(gyro_i(i)));


%% test modulate for all oasis functions.
col = {[0 114 178],[0 158 115], [213 94 0],[230 159 0],...
    [86 180 233], [204 121 167], [64 224 208], [240 228 66]}; % colors
plot_cvx = false;

%% example 1:  foopsi, AR1 model. This model is used when the sampling rate is low
g = 0.95;         % AR coefficient
noise = .3;
T = 3000;
framerate = 30;
firerate = 0.5;
b = 0;              % baseline
N = 1;              % number of trials
seed = 13;          % seed for genrating random variables
[y, true_c, true_s] = gen_data(g, noise, T, framerate, firerate, b, N, seed);

% case 1: all parameters are known
lambda = 2.4;
params.p = 1;
params.B = 1;
[c_oasis, s_oasis] = deconvolveCa(y, 'mcmc', params);  %#ok<*ASGLU>
[c_cvx, s_cvx] = foopsi(y, g, lambda);

figure('name', 'FOOPSI, AR1, known: g, lambda', 'papersize', [15, 4]);
plot_cvx = true;
show_results;
plot_cvx = false;


function M = threed_bilinear( kernel, phi, test, w_g )
%-----------------------------------------------------------------------
%  threed_bilinear.m - routine to compute \int{ kernel*phi*test }
%                      (same as twod_bilinear)
%
%  Copyright (c) 2002, Jeff Borggaard, Virginia Tech
%  Version: 1.0
%
%  Usage:    M = threed_bilinear(kernel, phi, test, w_g)
%
%  Variables:     kernel
%                        Kernel function in the integral evaluated
%                        at the Gauss points
%
%                 phi
%                        matrix of element test functions evaluated
%                        at the Gauss points (dim: n_gauss, n_dof)
%
%                 test
%                        matrix of test functions evaluated at the
%                        Gauss points (dim: n_gauss, n_dof)
%
%                 w_g
%                        Row vector of Gauss weights
%-----------------------------------------------------------------------
%   [n_gauss,n_row] = size(test);
%   [n_g1   ,n_col] = size(phi );
%
%   M = zeros(n_row,n_col);
%   for i=1:n_row
%     for j=1:n_col
%        M(i,j) = ( w_g'    .* test(:,i)' ) * ( kernel .* phi(:,j) );
%     end
%   end

  M = test'*diag( w_g.*kernel )*phi;


function feature = mk_mfcc(x,use_logE, fs)
if nargin<3
    fs = 8000;
end
if nargin<2
    use_logE = 0;
end

feature = fbank2mfcc(wav2fbank(x, fs));
if use_logE == 1
    feature(:,13) = comp_logE(x);
end
feature(:,14:26) = comp_delta(feature,3);
feature(:,27:39) = comp_delta(feature(:,14:26),2);


function [SenErrDef, IniErrDef, ObsErrDef, ini_pva, wander, dt]=sys_path(dir_name,record)
rstat=[134 26 345 476 5456;654 7345 8345 9678 140];       %control how random the errors are generated
%rstat=rand(2,5)*30000;
dt=1/100;
D2R=pi/180;

%%%imu error parameters (discrete time param. defined at 100Hz)
%Accelerometers (defined in m-sec^2)
SenErrDef(1).A=1;
SenErrDef(1).B=3e-006;
SenErrDef(1).C=1;
SenErrDef(1).D=1e-3/sqrt(0.01);
SenErrDef(1).sP=sqrt(5.1e-007);
SenErrDef(1).tparam=[];

SenErrDef(2)=SenErrDef(1);
SenErrDef(3)=SenErrDef(1);

%Gyroscopes (defined in rad/sec)
SenErrDef(4).A=0.999980000199999;
SenErrDef(4).B=1.9999800001658e-006;
SenErrDef(4).C=1;
SenErrDef(4).D=0.0006/sqrt(0.01);
SenErrDef(4).sP=sqrt(1e-007);
SenErrDef(4).tparam=[];  %first param=sP of temperature scale factor (RC), second=B of temperature RW

SenErrDef(5)=SenErrDef(4);
SenErrDef(6)=SenErrDef(4);

% SenErrDef(6).A=1;
% SenErrDef(6).B=0;
% SenErrDef(6).C=1;
% SenErrDef(6).D=0;
% SenErrDef(6).sP=0;
% SenErrDef(1)=SenErrDef(6);
% SenErrDef(2)=SenErrDef(6);
% SenErrDef(3)=SenErrDef(6);
% SenErrDef(4)=SenErrDef(6);
% SenErrDef(5)=SenErrDef(6);

%%initial err defs
IniErrDef.pos_sP=diag([1/6e6,1/6e6,1]);
IniErrDef.vel_sP=eye(3)*0.01;
IniErrDef.att_sP=eye(3)*(10*pi/180);
IniErrDef.wander_sP=eye(2)*2;

%3D simulation in NED
%initial nav values
vel_b=zeros(3,1);
att_n=[-3;5;110]*D2R;
pos_n=[39.88864*D2R;32.78002*D2R;1102]; %51.081755-114.1360749

%%observation err defs
[Rn, Re, g, sL, cL, WIE_E]=geoparam_v000(pos_n);
Sn=Rn+pos_n(3);
Se=(Re+pos_n(3))*cos(pos_n(1));
ObsErrDef.sR=diag([0.5/Sn, 0.5/Se, 1.5]); %position error std (m)

%%%%%%%%%%% Generate path %%%%%%%%%%%%%%%%
if (record)
    %%motion definitions
    mot_def(1,:)=[1 0 0 0 0 5];
    mot_def(2,:)=[5 0 0 0 2 10];
    mot_def(3,:)=[1 0 0 0 0 5];
    mot_def(4,:)=[3 0 0 90*D2R 0 15];
    mot_def(5,:)=[1 0 0 0 0 5];
    mot_def(6,:)=[3 0 0 90*D2R 0 15];
    mot_def(7,:)=[1 0 0 0 0 5];
    mot_def(8,:)=[3 0 0 -90*D2R 0 15];
    mot_def(9,:)=[1 0 0 0 0 10];
    mot_def(10,:)=[3 0 0 -90*D2R 0 15];
    mot_def(11,:)=[1 0 0 0 0 5];
    mot_def(12,:)=[5 0 0 0 0 10];

    PathGen_v002(dir_name, [pos_n, vel_b, att_n], mot_def, [1 1/dt;1 0.1], 0, [], [], [], rstat(:,1));

    % %%%Add error
    % %Imu errors
    AddIMUErr_v000([dir_name 'mimu.bin'], [dir_name 'imu.bin'], [dir_name 'imuerr.bin'], 7, 2:7, SenErrDef, [], rstat(:,2));
    AddObsErr_v000([dir_name 'gps.bin'], 7, [dir_name 'obs_pos.bin'], [5 6 7], ObsErrDef, rstat(:,3));

    %AddIMUErr_v000([dir_name 'mimu.bin'], [dir_name 'imu.bin'], [dir_name 'imuerr.bin'], 7, 2:7, [], [], rstat(:,2));
    %AddObsErr_v000([dir_name 'gps.bin'], 7, [dir_name 'obs_pos.bin'], [5 6 7], [], rstat(:,3));
end

%initialization errors
randn('state',rstat(:,4));
pos_err=IniErrDef.pos_sP*randn(3,1);
vel_err=IniErrDef.vel_sP*randn(3,1);
att_err=IniErrDef.att_sP*randn(3,1);
Cerr=euler2dcm_v000(att_err);
Cbg=euler2dcm_v000(att_n);
Cbg_err=Cerr'*Cbg;
att_ini=dcm2euler_v000(Cbg_err);

ini_pva=[pos_n+pos_err vel_b+vel_err att_ini];  %%Attitude will be initialized using acc data
%wander=[sin(att_ini(3));cos(att_ini(3))];
wander=[sin(105*pi/180);cos(105*pi/180)];


function out=mpeuler(precision)

if nargin==0
 mp_defaults
 precision=default_precision;
end

out_rval=mpfr_euler(precision);

out=class(struct('rval',out_rval,...
                  'ival','0',...
                  'precision',precision),'mp');


function V = LP3cvx(J, model)

n = size(model.S,2);

cvx_begin quiet

    variable v(n);

    maximize (ones(1,numel(J)) * v(J) );

    model.S*v==0;
    v>=model.lb;
    v<=model.ub;

cvx_end

V = v;


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: SSD versus translations, linearInter, HNSP level=4
%
%==============================================================================

clear, close all, help(mfilename)

setup2DHNSPData;
level = 4; m = ML{level}.m;
imgModel('set','imgModel','linearInter');
[T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega);
xc    = getCellCenteredGrid(omega,m);
Rc    = imgModel(R,omega,xc);

trafo('set','trafo','translation2D');
figure(1); clf;
[w1,w2] = ndgrid(0.2*linspace(-1,1,21),0.2*linspace(-1,1,21));
dc  = zeros(size(w1));
for j=1:numel(dc),
  yc = trafo([w1(j);w2(j)],xc);
  Tc = imgModel(T,omega,yc);
  dc(j) = SSD(Tc,Rc,omega,m);
  viewImage(Tc,omega,m); FAIRpause(1/100)
end;
figure(1); clf; surf(w1,w2,dc); hold on; grid off; contour(w1,w2,dc)
title(sprintf('translation, m=[%d,%d]',m)); view(-135,33);
%==============================================================================


function [vehicle_pose]=apply_kinematics(vehicle_pose,vehicle_m,rad_speed,deltaT)

vehicle_pose=vehicle_pose+[vehicle_m*cos(vehicle_pose(3))*deltaT; ...
    vehicle_m*sin(vehicle_pose(3))*deltaT; rad_speed*deltaT];
end


function varargout = dtmfdet(x,fs,varargin)
% DTMFDET   DTMF Tone Detection Using Goertzel Algorithm.
%
%  Y = DTMFDET(X,FS) returns the DTMF coded sequence
%  in present vector X. The sound sample frequency
%  is given by FS.
%
%  Y = DTMFDET(X,FS,'strong') uses a more robust algorithm
%  to detect two equall DTMF signals in a sequence. Some times
%  not using this strong and longer method can result in
%  only one DTMF signal detected, instead of the two.
%

LIM = .7; % Limit for detecting tone (if the intensity of the freq from goertzel is less the LIM* the max intensity, the tone wont be detected)
T = ['1' '2' '3' 'A' '4' '5' '6' 'B' '7' '8' '9' 'C' '*' '0' '#' 'D'];
FL = [697 770 852 941];
FH = [1209 1336 1477 1633];
OL = .0; % Overlapping block in goertzel.

pl = 0; % Plotting flag

for i = 1 : length(varargin)
    if strcmp(varargin{i},'strong')
        OL = .4;
    elseif strcmp(varargin{i},'plot')
        pl = 1;
    else
        error('Unknown parameter.')
    end
end

time = 0.02;
N = time*fs;

ADV = max(round((1-OL)*N),1);
YL = zeros(4,floor(length(x)/ADV));
YH = YL;

for i = 1 : 4
    YL(i,:) = goertzel(x,fs,FL(i),N,'mag','overlap',OL);
    YH(i,:) = goertzel(x,fs,FH(i),N,'mag','overlap',OL);
end

[a,a] = sort(YL,1);
[b,b] = sort(YH,1);

YL(YL<LIM*max(YL(:))) = 0;
YH(YH<LIM*max(YH(:))) = 0;

[l1,l2] = max(YL);
[h1,h2] = max(YH);

r = T((l2-1)*4+h2);
r(l1==0) = '-';
r(h1==0) = '-';

if pl == 1
    plot(YL')
    figure(gcf+1)
    plot(YH')
end

ct = 1;
for i = 2 : length(r)
    if r(i)~='-' & r(i)~=r(i-1)
        y(ct) = r(i);
        ct = ct+1;
    end
end

varargout{1} = y;


function [y, yvar, gene, times, scale, rawExp, rawVar] = drosGetData(drosexp, genes, getmedians),

% DROSGETDATA Get Drosophila data as processed by mmgMOS.
% FORMAT
% DESC Extract given genes from drosexp structure.
% ARG drosexp : drosexp structure as returned by drosLoadData
% ARG genes : indices or gene labels to extract
% ARG getmedians : return median values and zero variances
% RETURN y : the normalised expression levels.
% RETURN yvar : the variance of the normalised expression levels.
% RETURN gene : the gene names and Affymetrix array tags.
% RETURN times : the times of the expression measurements.
% RETURN scale : the scaling factor applied to normalise.
% RETURN rawExp : the raw gene expresion level.
% RETURN rawVar : the raw variance of the gene expression.
%
% SEEALSO : drosLoadData
%
% COPYRIGHT : Neil D. Lawrence, 2006
% COPYRIGHT : Antti Honkela, 2007

% SHEFFIELDML

if nargin < 3,
  getmedians = 0;
end

if iscell(genes),
  genes = drosFindGeneinds(drosexp, genes, 0, 1);
end

N = length(genes);

gene = drosexp.genes(genes);
if size(gene, 1) < size(gene, 2),
  gene = gene';
end
gene = [gene, gene];

rawExp = zeros(36, N);
rawVar = zeros(36, N);
yFull = zeros(36, N);
yFullVar = zeros(36, N);

rawExp = drosexp.mean(genes, :)';
rawVar = (drosexp.se(genes, :)').^2;
if getmedians,
  yFull = exp(drosexp.pctiles(genes, :, 3))';
  yFullVar = zeros(size(yFull));
else
  if isfield(drosexp, 'fitmean'),
    yFull = drosexp.fitmean(genes, :)';
    yFullVar = drosexp.fitvar(genes, :)';
  else
    for k=1:N,
      prof = squeeze(drosexp.pctiles(genes(k), :, :));
      for l=1:36,
	t = distfit(exp(prof(l, :)), 'normal');
	yFull(l, k) = t(1);
	yFullVar(l, k) = t(2) .^ 2;
      end
    end
  end
end

% Rescale so that average standard deviation of curves is 1.
scale = sqrt(var(yFull));
scaleMat = ones(size(yFull, 1), 1)*scale;
yFull = yFull./scaleMat;
yFullVar = yFullVar./(scaleMat.*scaleMat);

y{1} = yFull(1:12, :);
y{2} = yFull(13:24, :);
y{3} = yFull(25:36, :);
yvar{1} = yFullVar(1:12, :);
yvar{2} = yFullVar(13:24, :);
yvar{3} = yFullVar(25:36, :);
times = (1:12)';
%save('./data/barencoData.mat', 'y', 'yvar', 'gene', 'times', 'scale', 'rawVar', 'rawExp');


function make_srrc_lut
    OS_RATE = 8;

    f = firrcos(2*OS_RATE,.25,.25,1,'rolloff','sqrt');
    f = f/sum(abs(f)); % make sure no matter what we don't go beyond 1

    fid = fopen('SRRC.m','w+');
    fprintf(fid,'function y = SRRC\n');
    fprintf(fid,'%%#codegen\n');
    fprintf(fid,'y = [\n');
    fprintf(fid,'%13.12f\n',f);
    fprintf(fid,'];\n');
    fclose(fid);
end


 function g = huber_dpot(t, d)
%function g = huber_dpot(t, d)
% huber potential derivative function
g = t;
ii = abs(t) > d;
g(ii) = d * sign(t(ii));


%% (Internal) Filtered calculation of RR time series from QRS detections
%
% This function calculates a gap-removed version of the RR interval
% sequence.
%
%       RR = RR_calculation(QRS_detections, sampling_rate)
%
% Arguments:
%
%	   QRS_detections: the QRS detections
%
%	   sampling_rate: the sampling frequency of the detections.
%
% Example
%
% See also resample_sequences, MedianFiltSequence
%
% Author: Mariano Llamedo Soria (llamedom at frba.utn.edu.ar)
% Version: 0.1 beta
% Birthdate  : 25/4/2017
% Last update: 25/4/2017
% Copyright 2008-2017
%
function [ RR, RR_filt] = RR_calculation(QRS_detections, sampling_rate, RR_filt)

    RR = diff(QRS_detections);
    RR = [ RR(1,:); RR ];

    if( nargin < 3 || length(RR_filt) ~= length(QRS_detections) )
        % resample the RR sequence. Heavy in computation
        RR_filt = MedianFiltSequence(QRS_detections, RR, 2*sampling_rate);
    end

    % remove gaps using next heartbeats
    gap_relative_time = 3; % times the median RR interval
    aux_val = RR ./ RR_filt;
    aux_idx = find(aux_val >= gap_relative_time);

    for ii = rowvec(aux_idx)

        aux_idx = find( QRS_detections > QRS_detections(ii) & aux_val < gap_relative_time, 1, 'first');

        if ( isempty(aux_idx) )
            aux_idx = find( QRS_detections < QRS_detections(ii) & aux_val < gap_relative_time, 1, 'last');
        end

        RR(ii) = RR( aux_idx );

    end


classdef Results
% A class that produces various measures describing system accuracy.

properties (SetAccess = private)
  tar
  non
  numtar
  numnon
  Pmiss
  Pfa
  eer
  pb
end

methods
  % res = Results(scr,key);
  % res = Results(tar,non);
  function res = Results(param1,param2)
    if isnumeric(param1)
        assert(isnumeric(param2))
        res.tar = param1;
        res.non = param2;
    else
        scr = param1;
        key = param2;
        assert(isa(scr,'Scores'))
        assert(isa(key,'Key'))
        [res.tar, res.non] = scr.get_tar_non(key);
    end
    [res.Pmiss,res.Pfa] = rocch(res.tar,res.non);
    res.eer = rocch2eer(res.Pmiss,res.Pfa);
    res.numtar = length(res.tar);
    res.numnon = length(res.non);
    Nmiss = res.Pmiss * res.numtar;
    Nfa = res.Pfa * res.numnon;
    res.pb = rocch2eer(Nmiss,Nfa);
  end
  function actdcf = get_act_dcf(res,prior)
    actdcf = fast_actDCF(res.tar,res.non,logit(prior),false);
  end
  function mindcf = get_min_dcf(res,prior)
    Ptar = prior;
    Pnon = 1-prior;
    cdet = [Ptar,Pnon]*[res.Pmiss(:)';res.Pfa(:)'];
    mindcf = min(cdet,[],2);
  end
  function actdcf = get_norm_act_dcf(res,prior)
    actdcf = fast_actDCF(res.tar,res.non,logit(prior),true);
  end
  function mindcf = get_norm_min_dcf(res,prior)
    Ptar = prior;
    Pnon = 1-prior;
    cdet = [Ptar,Pnon]*[res.Pmiss(:)';res.Pfa(:)'];
    mindcf = min(cdet,[],2) ./ min(Ptar,Pnon);
  end
  function pb = get_prbep(res)
    pb = res.pb;
  end
  function eer_val = get_eer(res)
    eer_val = res.eer;
  end
  function ntar = num_tar(res)
    ntar = res.numtar;
  end
  function nnon = num_non(res)
    nnon = res.numnon;
  end
end

end


function SO3F = dot(SO3VF1, SO3VF2, varargin)
% pointwise inner product
%
% Syntax
%   SO3F = dot(SO3VF1, SO3VF2)
%   SO3F = dot(v, SO3VF2)
%   SO3F = dot(SO3VF1, v)
%
% Input
%   SO3VF1, SO3VF2 - @SO3VectorField
%   v - @vector3d
%
% Output
%   SO3F - @SO3Fun
%

if isa(SO3VF2, 'vector3d')
  SO3F = SO3VF1.SO3F;
  v = SO3VF2;
  SO3F = reshape(sum( SO3F .* reshape(v.xyz.',[3,size(v)]),1),size(v));
  if SO3VF2.antipodal
    SO3F = abs(SO3F);
  end
  return
end

if isa(SO3VF1, 'vector3d')
  SO3F = dot(SO3VF2,SO3VF1);
  return
end

ensureCompatibleSymmetries(SO3VF1,SO3VF2)
f = SO3FunHandle(@(rot) dot(SO3VF1.eval(rot),SO3VF2.eval(rot)),SO3VF1.CS,SO3VF1.SS);
SO3F = SO3FunHarmonic(f, varargin{:});

end


function [F,h,x] = sedumi2yalmip(At,b,c,K)

nvars = length(b);
x = sdpvar(nvars,1);

if size(At,2)~=length(b)
    At = At';
end

F = ([]);
top = 1;

if isvalidfield(K,'f')
    X = c(top:top+K.f-1)-At(top:top+K.f-1,:)*x;
    F = F + (X(:) == 0);
    top = top + K.f;
end

if isvalidfield(K,'l')
    X = c(top:top+K.l-1)-At(top:top+K.l-1,:)*x;
    F = F + (X(:)>=0);
    top = top + K.l;
end

if isvalidfield(K,'q')
    for i = 1:length(K.q)
        X = c(top:top+K.q(i)-1)-At(top:top+K.q(i)-1,:)*x;
        F = F + (cone(X(2:end),X(1)));
        top = top + K.q(i);
    end
end

if isvalidfield(K,'r')
    for i = 1:length(K.r)
        X = c(top:top+K.r(i)-1)-At(top:top+K.r(i)-1,:)*x;
        F = F + (rcone(X(3:end),X(2),X(1)));
        top = top + K.r(i);
    end
end

if isvalidfield(K,'s')
    for i = 1:length(K.s)
        [ix,iy,iv] = find([c(top:top+K.s(i)^2-1) At(top:top+K.s(i)^2-1,:)]);
        off = (ix-1)/(K.s(i)+1);
        if all(off == round(off))
            X = c(top:top+K.s(i)^2-1)-At(top:top+K.s(i)^2-1,:)*x;
            if isa(X,'sdpvar')
                F = F + (diag(reshape(X,K.s(i),K.s(i))) >= 0);
            else
                X
                i
                'silly data!'
            end
            top = top + K.s(i)^2;
        else
            X = c(top:top+K.s(i)^2-1)-At(top:top+K.s(i)^2-1,:)*x;
            X = reshape(X,K.s(i),K.s(i));
            X = (X+X')/2;
            F = F + (X >= 0);
            top = top + K.s(i)^2;
        end
    end
end

h = -b'*x;

function ok = isvalidfield(K,fld)
ok = 0;
if isfield(K,fld)
    s = getfield(K,fld);
    if prod(size(s))>0
        if s(1)>0
            ok = 1;
        end
    end
end


function test_bug2558

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

nsubj = 23;
nchan = 274;
ntime = 1560;

gavg_erf7.trial = randn(nsubj,nchan,ntime);
%gavg_erf7.individual = randn(nsubj,nchan,ntime);
gavg_erf7.time = (1:ntime)/1000;
for i=1:nchan
  gavg_erf7.label{i} = num2str(i);
end
gavg_erf7.dimord = 'subj_chan_time';
%gavg_erf7.avg = randn(nchan, ntime);
gavg_erf7.cfg = [];

gavg_erf8.trial = randn(nsubj,nchan,ntime);
%gavg_erf8.individual = randn(nsubj,nchan,ntime);
gavg_erf8.time = (1:ntime)/1000;
for i=1:nchan
  gavg_erf8.label{i} = num2str(i);
end
gavg_erf8.dimord = 'subj_chan_time';
%gavg_erf8.avg = randn(nchan, ntime);
gavg_erf8.cfg = [];

cfg = [];
cfg.channel = 'all'; % modification w.r.t. original bug report
cfg.latency = [0.4 0.7];
cfg.avgovertime = 'no';
cfg.avgoverchan = 'no';
%cfg.avgoverfreq = 'yes'; % <--- is this necessary?
cfg.parameter = 'trial';
cfg.method = 'montecarlo';
%cfg.correctm = 'cluster';
cfg.statistic = 'ft_statfun_depsamplesT';
cfg.alpha = 0.05;
%cfg.clusteralpha = 0.05;
%cfg.neighbours  = []; % modification w.r.t. original bug report
cfg.correctm    = 'no';
cfg.correcttail = 'prob';
cfg.numrandomization = 1000;

cfg.design(1,1:2*nsubj) =  [ones(1,nsubj) 2*ones(1,nsubj)];
cfg.design(2,1:2*nsubj) = [1:nsubj 1:nsubj];
cfg.ivar                = 1; % the 1st row in cfg.design contains the independent variable
cfg.uvar                = 2; % the 2nd row in cfg.design contains the subject numbe

stat = ft_timelockstatistics(cfg, gavg_erf7,gavg_erf8);


function [alpha_fin,b_fin] = rsptrain(K,Y,C,T,S_min),
% Recursive Stabilization Procedure
%% Variable Description
%%
%%
m = size(K,1);
k = ceil(log(m/S_min)/log(T/(T-1)));
current_position = [0,0];
Node_done = zeros(k,T);
Current_Path = zeros(k*T,m);
not_finished = 1;
count = 0;
Path_from_root = zeros(k,1);
while not_finished,

    cp1 = current_position(1);
    cp2 = current_position(2);

    if (cp1~=k),
        active = find(Node_done(cp1+1,1:T)==0);
    else
        active = [];
    end;%if (cp1~=k)

    if ~isempty(active),

        % The part of the execution tree is not completely explored
        if cp1==0,
            tmp=[1:m];
        else
            tmp = find(Current_Path(cp2 + (cp1-1)*T,:)==1);
        end; % if cp1==0,

        % Build the training set for the node
        tab_tmp = zeros(1,m);
        train_ind_tmp = tmp([1:floor((active(1)-1)*length(tmp)/T),floor(active(1)*length(tmp)/T):length(tmp)]);
        %% If all the points are from the same class then do not go further
        is_plus = find(Y(train_ind_tmp)==1);

        if (length(is_plus)==0|(length(is_plus)==length(train_ind_tmp))),
            Node_done(cp1+1,1:T)=1;
            Current_Path([1:T] + (cp1)*T,:) = zeros(T,m);
        else
            tab_tmp(train_ind_tmp)=1;
            current_position(1) = cp1+1;
            Path_from_root(current_position(1)) = current_position(2);
            current_position(2) = active(1);
            Current_Path(current_position(2) + (current_position(1)-1)*T,:) = tab_tmp;
        end;
    else
        if (cp1==0),
            tmp = [1:m];
        else
            tmp = find(Current_Path(current_position(2)+(current_position(1)-1)*T,:)==1);
        end;
            Ylearn = Y(tmp,:);
            Clearn = (C*m)/(length(tmp));
            Klearn = K(tmp,tmp);

        if current_position(1)==k,
            [alpha] = quadsolve(Klearn,-ones(length(tmp),1),Ylearn',0,Clearn);
            alpha_bias=alpha;
            scale_f = sqrt(alpha'*Klearn*alpha);
            alpha = alpha/scale_f;
        else
            c = Klearn*(Current_Path(current_position(1)*T+1:current_position(1)*T+T,tmp))';
            c = mean(c,2);
            c = -1+c;
            H = 1/(T)*Klearn;
            [alpha,y] = quadsolve(H,c,Ylearn',0,Clearn);
            alpha_bias = alpha;
            alpha = 1/T*(alpha + sum(Current_Path(current_position(1)*T+1:current_position(1)*T+T,tmp)',2));
            scale_f = sqrt(alpha'*Klearn*alpha);
            alpha = alpha/scale_f;
        end; % if current_pos...==k-1

        if current_position ~=0,
            Current_Path(current_position(2)+(current_position(1)-1)*T,tmp) = alpha';
            Node_done(current_position(1)+1,1:T)=0;

            Node_done(current_position(1),current_position(2)) = 1;
            count=count+1;
            current_position(2) = Path_from_root(current_position(1));
            current_position(1) = current_position(1)-1;
        else,
            not_finished=0;
            alpha_fin = alpha;
            b_fin = -y/scale_f;
        end; % if current_pos...~=0

    end;% if ~isempty(active)

%    disp(sprintf('Node done: %d\n',count));

end; %while not_finished
% End function


function [BB vx] = spm_get_bbox(V, thr, premul)
% Compute volume's bounding box, for full field of view or object bounds
% FORMAT [BB vx] = spm_get_bbox(V, thr)
% V   - mapped image volume(s) (from spm_vol) or filename (empty for GUI)
% thr - threshold, such that BB contains voxels with intensities > thr
%       or strings 'nz', 'nn', fv', for non-zero, non-NaN, or field of view
%       where 'fv' (the default) uses only the image's header information.
%
% BB  - a [2 x 3] array of the min and max X, Y, and Z coordinates {mm},
%       i.e. BB = [minX minY minZ; maxX maxY maxZ].
% vx  - a [1 x 3] vector of voxel dimensions {mm}.
%__________________________________________________________________________
% Copyright (C) 2011 Wellcome Trust Centre for Neuroimaging

% Ged Ridgway
% $Id: spm_get_bbox.m 4205 2011-02-21 15:39:08Z guillaume $

% Undocumented expert options:
% V           - can be a 4D @nifti object (but not 5D), image-based BBs
%               will be computed using "all" along the 4th dimension.
% thr = 'old' - reproduce spm_write_sn/bbvox_from_V (and elsewhere)
% premul      - a matrix that premultiplies V.mat, as used in spm_orthviews

%-Get an SPM volume structure
%--------------------------------------------------------------------------
if nargin < 1 || isempty(V)
    [V, sts] = spm_select(1, 'image', 'Select Image');
    if ~sts, error('Must select an image'), end
end
if ischar(V), V = spm_vol(V); end

%-Get volume structure from @nifti object if given
%--------------------------------------------------------------------------
if isa(V, 'nifti')
    V = spm_vol(V.dat.fname); % (potentially a struct array of volumes)
end

%-Compute voxel dimensions (for compatibility with bbvox_from_V)
%--------------------------------------------------------------------------
P = spm_imatrix(V(1).mat);
vx = P(7:9);
% the above agrees with sqrt(sum(V.mat(1:3,1:3).^2)) for simple rotations,
% and seems more appropriate if there are reflections and/or skews.
% Note that spm_imatrix(diag([-1 1 1 1])) is [-1 1 1] as expected.

%-Compute bounding box
%--------------------------------------------------------------------------
if nargin < 2 || isempty(thr) || strcmpi(thr, 'fv')
    % overall field-of-view bounding box from header information
    d = V(1).dim;
    corners = [
        1    1    1    1
        1    1    d(3) 1
        1    d(2) 1    1
        1    d(2) d(3) 1
        d(1) 1    1    1
        d(1) 1    d(3) 1
        d(1) d(2) 1    1
        d(1) d(2) d(3) 1
        ]';
    XYZ = V(1).mat(1:3, :) * corners;
elseif strcmpi(thr, 'old')
    % code from spm_write_sn/bbvox_from_V (and other places)
    % NB: main difference is that vx(1)<0 gives descending BB(:,1),
    % shouldn't be used if V.mat contains rotations or skews.
    o  = V(1).mat\[0 0 0 1]';
    o  = o(1:3)';
    BB = [-vx.*(o-1) ; vx.*(V(1).dim(1:3)-o)];
    if exist('premul', 'var')
        warning('spm_get_bbox:old_and_premul', 'old method ignores premul')
    end
else
    % image-based bounding box using voxel intensities
    [img XYZ] = spm_read_vols(V);
    if ischar(thr)
        switch lower(thr)
            case 'nn'  % non-NaN, though include +/- Inf in computation
                img = ~isnan(img);
            case 'nz'  % special case of non-zero (rather than > 0)
                img = ~isnan(img) & img ~= 0;
            otherwise
                error('Unknown threshold type %s', thr)
        end
    else
        % treat thr as numeric threshold
        img = img > thr;
    end
    if ndims(img) == 4
        img = all(img, 4);
    end
    if nnz(img) == 0
        warning('spm_get_bbox:nothing', ...
            'Threshold leaves no voxels, returning full field of view');
    else
        XYZ = XYZ(:, img); % keep only coords that satisfy condition
    end
end

if ~exist('BB', 'var') % exists already if 'old' case chosen above
    if exist('premul', 'var')
        XYZ = premul(1:3, :) * [XYZ; ones(1, size(XYZ, 2))];
    end
    BB = [
        min(XYZ, [], 2)'
        max(XYZ, [], 2)'
        ];
end


%% Copyright (C) 2014-2016, 2019 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 int (@var{f})
%% @defmethodx @@sym int (@var{f}, @var{x})
%% @defmethodx @@sym int (@var{f}, @var{x}, @var{a}, @var{b})
%% @defmethodx @@sym int (@var{f}, @var{x}, [@var{a}, @var{b}])
%% @defmethodx @@sym int (@var{f}, @var{a}, @var{b})
%% @defmethodx @@sym int (@var{f}, [@var{a}, @var{b}])
%% Symbolic integration.
%%
%% Definite integral:
%% @example
%% @group
%% syms x
%% f = x^2;
%% F = int(f, x, 1, 2)
%%   @result{} F = (sym) 7/3
%% @end group
%% @end example
%% or alternatively
%% @example
%% @group
%% F = int(f, x, [1 2])
%%   @result{} F = (sym) 7/3
%% @end group
%% @end example
%%
%% Indefinite integral:
%% @example
%% @group
%% F = int(f, x)
%%   @result{} F = (sym)
%%        3
%%       x
%%       ──
%%       3
%% F = int(f)
%%   @result{} F = (sym)
%%        3
%%       x
%%       ──
%%       3
%% @end group
%% @end example
%%
%% @seealso{@@sym/diff}
%% @end defmethod

function F = int(f, x, a, b)

  if (nargin == 4)
    % int(f, x, a, b)
    assert(numel(a)==1)
    assert(numel(b)==1)
    definite = true;

  elseif (nargin == 2) && (numel(x) == 1)
    % int(f, x)
    definite = false;

  elseif (nargin == 1)
    % int(f)
    definite = false;
    x = symvar(f,1);
    if isempty(x)
      x = sym('x');
    end

  elseif (nargin == 2) && (numel(x) == 2)
    % int(f, [a b])
    idx.type = '()';
    idx.subs = {2};
    definite = true;
    b = subsref(x, idx);
    idx.subs = {1};
    a = subsref(x, idx);

    x = symvar(f,1);
    if isempty(x)
      x = sym('x');
    end

  elseif (nargin == 3) && (numel(a) == 2)
    % int(f, x, [a b])
    definite = true;
    idx.type = '()';
    idx.subs = {2};
    b = subsref(a, idx);
    idx.subs = {1};
    a = subsref(a, idx);

  elseif (nargin == 3) && (numel(a) == 1)
    % int(f, a, b)
    definite = true;
    b = a;
    a = x;
    x = symvar(f,1);
    if isempty(x)
      x = sym('x');
    end

  else
    print_usage ();

  end

  %% now do the definite or indefinite integral
  if (definite)
    cmd = { '(f, x, a, b) = _ins'
            'F = sp.integrate(f, (x, a, b))'
            'return F,' };
    F = pycall_sympy__ (cmd, sym(f), sym(x), sym(a), sym(b));
  else
    cmd = { '(f,x) = _ins'
            'd = sp.integrate(f, x)'
            'return d,' };
    F = pycall_sympy__ (cmd, sym(f), sym(x));
  end

end

%!shared x,y,a
%! syms x y a
%!assert(logical(int(cos(x)) - sin(x) == 0))
%!assert(logical(int(cos(x),x) - sin(x) == 0))
%!assert(logical(int(cos(x),x,0,1) - sin(sym(1)) == 0))

%!test
%! %% limits might be syms
%! assert( isequal (int(cos(x),x,sym(0),sym(1)), sin(sym(1))))
%! assert( isequal (int(cos(x),x,0,a), sin(a)))

%!test
%! %% other variables present
%! assert( isequal (int(y*cos(x),x), y*sin(x)))

%!test
%! %% limits as array
%! assert( isequal (int(cos(x),x,[0 1]), sin(sym(1))))
%! assert( isequal (int(cos(x),x,sym([0 1])), sin(sym(1))))
%! assert( isequal (int(cos(x),x,[0 a]), sin(a)))

%!test
%! %% no x given
%! assert( isequal (int(cos(x),[0 1]), sin(sym(1))))
%! assert( isequal (int(cos(x),sym([0 1])), sin(sym(1))))
%! assert( isequal (int(cos(x),[0 a]), sin(a)))
%! assert( isequal (int(cos(x),0,a), sin(a)))

%!test
%! %% integration of const
%! assert( isequal (int(sym(2),y), 2*y))
%! assert( isequal (int(sym(2)), 2*x))
%! assert( isequal (int(sym(2),[0 a]), 2*a))
%! assert( isequal (int(sym(2),0,a), 2*a))

%!test
%! % componentwise int of array
%! A = [x x*x];
%! assert (isequal (int(A, x), [x^2/2 x^3/3]))

%!test
%! % NonElementaryIntegral bug
%! % https://savannah.gnu.org/bugs/index.php?46831
%! f = int(exp(exp(x)));
%! f = f + 2;
%! g = diff(f);
%! assert (isequal (g, exp(exp(x))))


function X = rocholForeSub(ch, Y);

% ROCHOLFORESUB Foreward substitute the representation of the rank one Cholesky.

% ROCHOL

%/~
% This would be the long way of doing it.
%L = rocholExtract(ch);
%X2 = L\Y;
%~/

X = zeros(size(Y));
t = zeros(1, size(Y, 2));
X(1, :) = Y(1, :)/ch.s(1);
for i = 2:ch.n
  if i == 2
    t = Y(1, :)*ch.u(1);
  else
    t = t + ch.s(i-1)*ch.u(i-1)*X(i-1, :);
  end
  X(i, :) = (Y(i, :)-ch.v(i)*t)/ch.s(i);
end

%/~
%disp(max(max(X - X2)));
%~/


function [S,KL,KH] = use_spm_filter(TR,dims,LChoice,HChoice,HParam,varargin)
% :Usage:
% ::
%
%     function [S,KL,KH] = use_spm_filter(TR,dim of filter,LChoice,HChoice,HP filter in s,[LP Gauss len in s])
%
% :Inputs:
%
%   **K{s}.LChoice:**
%        Low-pass  filtering {'hrf' 'Gaussian' 'none'}
%   **K{s}.LParam:**
%        Gaussian parameter in seconds
%   **K{s}.HChoice:**
%        High-pass filtering {'specify' 'none'}
% ..
%    05/22/01 Tor Wager
% ..

K{1}.RT = TR;
K{1}.LChoice = LChoice;
K{1}.HChoice = HChoice;
K{1}.HParam = HParam;
K{1}.row = ones(dims, 1);

if length(varargin) > 0
        K{1}.LParam = varargin{1};
end

KL = []; KH = [];

spmS = spm_filter('set', K);

S = eye(length(K{1}.row));

if ~strcmp(HChoice,'none')

	KH = full(spmS{1}.KH);
	S = S - KH * pinv(KH);

end

if ~strcmp(LChoice,'none')
	KL = full(spmS{1}.KL);
	S = KL * S;         	% lowpass * highpass; hp = I - res forming mtx of S.KH
end

return

function [vargout] = spm_filter(Action,K,Y)
% filter routine
% FORMAT [K] = spm_filter('set',K)
% FORMAT [Y] = spm_filter('apply',K,Y)
%
% Action    - 'set'   fills in filter structure K
% Action    - 'apply' applies K to Y = K*Y
% K         - filter convolution matrix or:
% K{s}      - cell of structs containing session-specific specifications
%
% K{s}.RT       - repeat time in seconds
% K{s}.row      - row of Y constituting session s
% K{s}.LChoice  - Low-pass  filtering {'hrf' 'Gaussian' 'none'}
% K{s}.LParam   - Gaussian parameter in seconds
% K{s}.HChoice  - High-pass filtering {'specify' 'none'}
% K{s}.HParam   - cut-off period in seconds
%
% K{s}.HP       - low frequencies to be removed
% K{s}.LP       - sparse toepltz low-pass convolution matrix
%
% Y         - data matrix
%
% K         - filter structure
% Y         - filtered data K.K*Y
%___________________________________________________________________________
%
% spm_filter implements band pass filtering in an efficient way by
% using explicitly the projector matrix form of the High pass
% component.  spm_filter also configures the filter structure in
% accord with the specification fields if required
%___________________________________________________________________________
% @(#)spm_filter.m	2.4 Karl Friston 99/08/31

% set or apply
%---------------------------------------------------------------------------
switch Action

	case 'set'
	%-------------------------------------------------------------------
	for s = 1:length(K)

		% matrix order
		%-----------------------------------------------------------
		k     = length(K{s}.row);

		% make low pass filter
		%-----------------------------------------------------------
		switch K{s}.LChoice

			case 'none'
			%---------------------------------------------------
			h       = 1;
			d       = 0;

			case 'hrf'
			%---------------------------------------------------
			h       = spm_hrf(K{s}.RT);
			h       = [h; zeros(size(h))];
			g       = abs(fft(h));
			h       = real(ifft(g));
			h       = fftshift(h)';
			n       = length(h);
			d       = [1:n] - n/2 - 1;

			case 'Gaussian'
			%---------------------------------------------------
			sigma   = K{s}.LParam/K{s}.RT;
			h       = round(4*sigma);
			h       = exp(-[-h:h].^2/(2*sigma^2));
			n       = length(h);
			d       = [1:n] - (n + 1)/2;
			if      n == 1, h = 1; end

			otherwise
			%---------------------------------------------------
			error('Low pass Filter option unknown');
			return

		end

		% create and normalize low pass filter
		%-----------------------------------------------------------
		K{s}.KL = spdiags(ones(k,1)*h,d,k,k);
		K{s}.KL = spdiags(1./sum(K{s}.KL')',0,k,k)*K{s}.KL;

		% make high pass filter
		%-----------------------------------------------------------
		switch K{s}.HChoice

			case 'none'
			%---------------------------------------------------
			K{s}.KH = [];

			case 'specify'
			%---------------------------------------------------
			n       = fix(2*(k*K{s}.RT)/K{s}.HParam + 1);
			X       = spm_dctmtx(k,n);
			K{s}.KH = sparse(X(:,[2:n]));

			otherwise
			%---------------------------------------------------
			error('High pass Filter option unknown');
			return

		end

	end

	% return structure
	%-------------------------------------------------------------------
	vargout = K;

	case 'apply'
	%-------------------------------------------------------------------
	if iscell(K)

		% ensure requisite feild are present
		%-----------------------------------------------------------
		if ~isfield(K{1},'KL')
			K = spm_filter('set',K);
		end

		for s = 1:length(K)

			% select data
			%---------------------------------------------------
			y = Y(K{s}.row,:);

			% apply low pass filter
			%---------------------------------------------------
			y = K{s}.KL*y;

			% apply high pass filter
			%---------------------------------------------------
			if ~isempty(K{s}.KH)
				y = y - K{s}.KH*(K{s}.KH'*y);
			end

			% reset filtered data in Y
			%---------------------------------------------------
			Y(K{s}.row,:) = y;

		end

	% K is simply a convolution matrix
	%-------------------------------------------------------------------
	else
		Y = K*Y;
	end

	% return filtered data
	%-------------------------------------------------------------------
	vargout   = Y;

	otherwise
	%-------------------------------------------------------------------
	warning('Filter option unknown');

end

function C = spm_dctmtx(N,K,n,f)
% Creates basis functions for Discrete Cosine Transform.
% FORMAT C = spm_dctmtx(N,K,n)
%     OR C = spm_dctmtx(N,K)
%     OR D = spm_dctmtx(N,K,n,'diff')
%     OR D = spm_dctmtx(N,K,'diff')
% N - dimension
% K - order
% n - optional points to sample
%____________________________________________________________________________
% spm_dctmtx creates a matrix for the first few basis functions of a one
% dimensional discrete cosine transform.
% With the 'diff' argument, spm_dctmtx produces the derivatives of the
% DCT.
%
% See:    Fundamentals of Digital Image Processing (p 150-154).
%         Anil K. Jain 1989.
%____________________________________________________________________________
% @(#)spm_dctmtx.m	1.3 John Ashburner MRCCU/FIL 96/08/14

d = 0;

if (nargin == 2)
	n = (0:(N-1))';
	if (nargin == 3)
		d = 1;
	end
elseif (nargin == 3)
	if (strcmp(n,'diff'))
		d = 1;
		n = (0:(N-1))';
	else
		n = n(:);
	end
elseif (nargin == 4)
	n = n(:);
	if (strcmp(f,'diff'))
		d = 1;
	else
		error('Incorrect Usage');
	end
else
	error('Incorrect Usage');
end

C = zeros(size(n,1),K);

if (d == 0)
	C(:,1)=ones(size(n,1),1)/sqrt(N);
	for k=2:K
		C(:,k) = sqrt(2/N)*cos(pi*(2*n+1)*(k-1)/(2*N));
	end
else
	for k=2:K
		C(:,k) = -2^(1/2)*(1/N)^(1/2)*sin(1/2*pi*(2*n*k-2*n+k-1)/N)*pi*(k-1)/N;
	end
end


function [c] = cellcovshift(x, shift, dim, flag)

% [C] = CELLCOVSHIFT(X, SHIFT, DIM) computes the covariance, across all cells
% in x along the dimension dim.
%
% X should be linear cell-array(s) of matrices for which the size in at
% least one of the dimensions is be the same for all cells

if nargin<4,
  flag = 1;
end

if nargin<3,
  dim = find(size(x{1})>1, 1, 'first');
end

nx = size(x);
if ~iscell(x) || length(nx)>2 || all(nx>1) || ndims(x{1})>2,
  error('incorrect input for cellcovshift');
end

% shift the time axis
y = cellshift(x, shift, dim);

% compute covariance
c = cov(y, 1, dim, flag);
%c = cellnancov(y, 1, dim, flag);


function i4_is_prime_test ( )

%*****************************************************************************80
%
%% I4_IS_PRIME_TEST tests I4_IS_PRIME.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4_IS_PRIME_TEST\n' );
  fprintf ( 1, '  I4_IS_PRIME reports whether an integer is prime.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  I     I4_IS_PRIME(I)\n' );
  fprintf ( 1, '\n' );

  for i = -2 : 25
    fprintf ( 1, '  %6d  %1d\n', i, i4_is_prime ( i ) );
  end

  return
end


function MatingPool = MatingSelection(PopObj,Rank)
% The mating selection of one-by-one EA

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

    %% Calculate the density of each solution
    d  = pdist2(PopObj,PopObj,'cosine');
    d  = sort(d,2);
    dk = 1./(sum(d(:,2:ceil(end/10)),2)+1);

    %% Binary tournament selection
    MatingPool = TournamentSelection(2,size(PopObj,1),Rank,dk);
end


function [ZV,ZV2] = ZV_gen(Z,cz,Y,ind)
    [d n]= size(Z);
    b = size(cz,2);

    for ii = 1:2
        if ii == 1
            XX = Y;
        else
            XX = Z;
        end;
        cxx = XX(:,ind);
        sqX = sum(XX.^2,1);
        Xc = XX'*cxx;
        sqcx = sum(cxx.^2,1);
        ZV2{ii} = ones(n,1)*sqcx - 2*Xc + sqX'*ones(1,b);
    end;

    for l = 1:d
        ZV{l} = ones(n,1)*cz(l,:) - Z(l,:)'*ones(1,size(cz,2));
    end;


%ISTEREO Stereo matching
%
% D = ISTEREO(LEFT, RIGHT, RANGE, H, OPTIONS) is a disparity image computed
% from the epipolar aligned stereo pair: the left image LEFT (HxW) and the
% right image RIGHT (HxW).  D (HxW) is the disparity and the value at each
% pixel is the horizontal shift of the corresponding pixel in IML as observed
% in IMR. That is, the disparity d=D(v,u) means that the pixel at RIGHT(v,u-d)
% is the same world point as the pixel at LEFT(v,u).
%
% RANGE is the disparity search range, which can be a scalar for disparities in
% the range 0 to RANGE, or a 2-vector [DMIN DMAX] for searches in the range
% DMIN to DMAX.
%
% H is the half size of the matching window, which can be a scalar for NxN or a
% 2-vector [N,M] for an NxM window.
%
% [D,SIM] = ISTEREO(LEFT, RIGHT, RANGE, H, OPTIONS) as above but returns SIM
% which is the same size as D and the elements are the peak matching score
% for the corresponding elements of D.  For the default matching metric ZNCC
% this varies between -1 (very bad) to +1 (perfect).
%
% [D,SIM,DSI] = ISTEREO(LEFT, RIGHT, RANGE, H, OPTIONS) as above but returns DSI
% which is the disparity space image (HxWxN) where N=DMAX-DMIN+1. The I'th
% plane is the similarity of IML to IMR shifted to the left by DMIN+I-1.
%
% [D,SIM,P] = ISTEREO(LEFT, RIGHT, RANGE, H, OPTIONS) if the 'interp' option is
% given then disparity is estimated to sub-pixel precision using quadratic
% interpolation.  In this case D is the interpolated disparity and P is
% a structure with elements A, B, dx.  The interpolation polynomial is
% s = Ad^2 + Bd + C where s is the similarity score and d is disparity relative
% to the integer disparity at which s is maximum.  P.A and P.B are matrices the
% same size as D whose elements are the per pixel values of the interpolation
% polynomial coefficients.  P.dx is the peak of the polynomial with respect
% to the integer disparity at which s is maximum (in the range -0.5 to +0.5).
%
% Options::
% 'metric',M   string that specifies the similarity metric to use which is
%              one of 'zncc' (default), 'ncc', 'ssd' or 'sad'.
% 'interp'     enable subpixel interpolation and D contains non-integer
%              values (default false)
% 'vshift',V   move the right image V pixels vertically with respect to left.
%
% Example::
%
% Load the left and right images
%         L = iread('rocks2-l.png', 'reduce', 2);
%         R = iread('rocks2-r.png', 'reduce', 2);
% then compute stereo disparity and display it
%         d = istereo(L, R, [40, 90], 3);
%         idisp(d);
%
% References::
%  - Robotics, Vision & Control, Section 14.3,
%    P. Corke, Springer 2011.
%
% Notes::
% - Images must be greyscale.
% - Disparity values pixels within a half-window dimension (H) of the edges
%   will not be valid and are set to NaN.
% - The C term of the interpolation polynomial is not computed or returned.
% - The A term is high where the disparity function has a sharp peak.
% - Disparity and similarity score can be obtained from the disparity space
%   image by [SIM,D] = max(DSI, [], 3)
%
%
% See also IRECTIFY, STDISP.

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

function [disp,sim, o3] = istereo(L, R, drange, h, varargin)

% TODO: score cube is float, can return it

    opt.metric = 'zncc';
    opt.interp = false;
    opt.vshift = 0;

    opt = tb_optparse(opt, varargin);

    % ensure images are greyscale
    L = imono(L);
    R = imono(R);

    opt.vshift = round(opt.vshift);
    if opt.vshift ~= 0
        if opt.vshift > 0
            L = L(1:end-opt.vshift,:);
            R = R(opt.vshift:end,:);
        else
            vshift = -vshift;
            L = L(opt.vshift:end,:);
            R = R(1:end-opt.vshift,:);
        end
    end

    % compute the score cube, 3rd dimension is disparity
    DSI = stereo_match(L, R, 2*h+1, drange(1:2), opt.metric);

    % best value along disparity dimension is the peak
    %   s best score
    %   d disparity at which it occurs
    %
    % both s and d are matrices same size as L and R.
    if strcmp(opt.metric, 'sad') | strcmp(opt.metric, 'ssd')
        [s,d] = min(DSI, [], 3);
    else
        [s,d] = max(DSI, [], 3);
    end

    d(isnan(s)) = NaN;

    if opt.interp
        % interpolated result required

        % get number of pixels and disparity range
        npix = prod(size(L));

        if length(drange) == 1,
            ndisp = drange + 1;
        else
            dmin = min(drange);
            dmax = max(drange);
            ndisp = dmax - dmin + 1;
        end

        % find all disparities that are not at either end of the range, we need
        % a point on either side to interpolate them
        valid = (d>1) & (d<ndisp);
        valid = valid(:);

        % make a vector of consecutive pixel indices (1 to width*height)
        ci = [1:npix]';
        % turn disparities into a column vector
        dcol = d(:);

        % remove all entries that are not valid
        ci(~valid) = [];
        dcol(~valid) = [];

        % both ci and dcol have the same number of entries

        % for every valid pixel and disparity, find the index into the 3D score
        % array.  We cheat and consider that array WxHxD as a 2D array (WxH)xD
        %
        % We compute the indices for the best score and one each side of it
        k_m = sub2ind([npix ndisp], ci, dcol-1);
        k_0 = sub2ind([npix ndisp], ci, dcol);
        k_p = sub2ind([npix ndisp], ci, dcol+1);

        % initialize matrices (size of L and R) to hold the the best score
        % and the one each side of it
        y_m = ones(size(L))*NaN;
        y_0 = ones(size(L))*NaN;
        y_p = ones(size(L))*NaN;

        % now copy over the valid scores into these arrays.  What doesnt
        % get copies is a NaN
        y_m(ci) = DSI(k_m);
        y_0(ci) = DSI(k_0);
        y_p(ci) = DSI(k_p);

        % figure the coefficients of the peak fitting parabola:
        %    y = Ax^2 + Bx + C
        % Each coefficient is a matrix same size as (L and R)
        % We don't need to compute C
        A = 0.5*y_m - y_0 + 0.5*y_p;
        B = -0.5*y_m + 0.5*y_p;

        % now the position of the peak is given by -B/2A
        dx = -B ./ (2*A);

        % and we add this fractional part to the integer value obtained
        % from the max/min function
        d = d + dx;

   end
   d = d + drange(1)-1;

    if nargout > 0,
        disp = d;
    end
    if nargout > 1,
        sim = s;
    end

    if nargout > 2
        if opt.interp
            o3.A = A;
            o3.B = B;
            o3.dx = dx;
        else
            o3 = DSI;
        end
    end


function triangulation_test17 ( )

%*****************************************************************************80
%
%% TEST17 tests TRIANGULATION_ORDER3_EXAMPLE1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 August 2006
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST17\n' );
  fprintf ( 1, '  TRIANGULATION_ORDER3_EXAMPLE1_SIZE gives the sizes\n' );
  fprintf ( 1, '    for an example triangulation;\n' );
  fprintf ( 1, '  TRIANGULATION_ORDER3_EXAMPLE1 returns the information\n' );
  fprintf ( 1, '    for an example triangulation;\n' );
  fprintf ( 1, '  TRIANGULATION_ORDER3_PRINT prints a triangulation.\n' );
%
%  Get the sizes.
%
  [ node_num, triangle_num, hole_num ] = ...
    triangulation_order3_example1_size ( );
%
%  Get the data.
%
  [ node_xy, triangle_node, triangle_neighbor ] = ...
    triangulation_order3_example1 ( node_num, triangle_num );

  triangulation_order3_print ( node_num, triangle_num, node_xy, ...
    triangle_node, triangle_neighbor );

  return
end


function newnode=sms(node,face,iter,alpha,method)
%
% newnode=sms(node,face,iter,useralpha,method)
%
% simplified version of surface mesh smoothing
%
% author: Qianqian Fang, <q.fang at neu.edu>
% date: 2009/10/21
%
% input:
%    node:  node coordinates of a surface mesh
%    face:  face element list of the surface mesh
%    iter:  smoothing iteration number
%    alpha: scaler, smoothing parameter, v(k+1)=alpha*v(k)+(1-alpha)*mean(neighbors)
%    method: same as in smoothsurf, default is 'laplacianhc'
%
% output:
%    newnode: output, the smoothed node coordinates
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

if(nargin<5)
   method='laplacianhc';
end
if(nargin<4)
   if(nargin<3)
      iter=10;
   end
   alpha=0.5;
end

conn=meshconn(face,size(node,1));
newnode=smoothsurf(node(:,1:3),[],conn,iter,alpha,method,alpha);


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

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

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

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

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

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

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

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

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

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

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

%leave only the real part of the solutions
q=real(q);

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

A2 = abs(a(3));

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

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

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

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

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

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

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) =  atan(L3/ A2)-eta;
q3(2) =  atan(L3/ A2)+ eta;


function Y = slmulvec(X, v, d)
%SLMULVEC multiplies a vector to columns or rows of a matrix
%
% $ Syntax $
%   - Y = slmulvec(X, v, d)
%   - Y = slmulvec(X, v)
%
% $ Arguments $
%   - X:        The original matrix
%   - v:        The addend vector
%   - d:        The dimension along which the vector is to add
%   - Y:        The resultant matrix
%
% $ Description $
%   - Y = slmulvec(X, v, d) selects the most efficienct way to multiple a
%     vector v to every column/row of X. If d == 1, then v should be
%     a column vector, and is multiplied to each column of X, if d == 2,
%     then v should be a row vector, and is multiplied to each row of X.
%
%   - Y = slmulvec(X, v) will automatically determine d according to
%     the shape of v.
%
% $ Remarks $
%   - The implementation simply wraps the mex function vecop_core.
%
% $ History $
%   - Created by Dahua Lin, on Sep 10, 2006
%

if nargin < 3
    if size(v, 2) == 1
        d = 1;
    else
        d = 2;
    end
end

Y = vecop_core(X, v, d, 2);  % 2 is the opcode of multiplication in vecop_core


function [affine_matrix, transformed_matrix] = PTKSolveForRigidTranslation(image_to_transform, reference_image, reporting)
    % PTKSolveForRigidTranslation. Computes the transformation matrix to register one
    %     image segmentation to another based on an solving for a rigid
    %     translation.
    %
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    % Initial centroid registration
    [com_affine_matrix, com_affine_vector] = PTKRegisterCentroid(image_to_transform, reference_image, reporting);

    % Before computing a distance transform, we must resample the images so they are approximately isotropic
    % Find a voxel size to use for the registration image. We divide up thick
    % slices to give an approximately isotropic voxel size
    register_voxel_size = reference_image.VoxelSize;
    register_voxel_size = register_voxel_size./round(register_voxel_size/min(register_voxel_size));
    reference_image2 = reference_image.Copy;
    reference_image2.ResampleBinary(register_voxel_size);
    image_to_transform2 = image_to_transform.Copy;
    image_to_transform2.ResampleBinary(register_voxel_size);

    reference_image2.AddBorder(20);
    image_to_transform2.AddBorder(20);

    dt_float = PTKRunForEachComponentAndCombine(@MimImageUtilities.GetNormalisedDT, image_to_transform2, image_to_transform2, reporting);
    dt_ref = PTKRunForEachComponentAndCombine(@MimImageUtilities.GetNormalisedDT, reference_image2, reference_image2, reporting);

    dt_float.RescaleToMaxSize(128);

    dt_ref.RescaleToMaxSize(128);

    [affine_matrix, transformed_matrix] = Solve(dt_float, dt_ref, com_affine_vector(4:6), reporting);
end

function [affine_matrix, transformed_matrix] = Solve(image_to_transform, reference_image, starting_affine_vector, reporting)
    [i_o, j_o, k_o] = image_to_transform.GetCentredGlobalCoordinatesMm;
    [i_o, j_o, k_o] = ndgrid(i_o, j_o, k_o);

    [i_r, j_r, k_r] = reference_image.GetCentredGlobalCoordinatesMm;
    [i_r, j_r, k_r] = ndgrid(i_r, j_r, k_r);

    x_0 = starting_affine_vector';
    AnonFn = @(x) FnToMinimiseRigid(x, image_to_transform, reference_image, i_o, j_o, k_o, i_r, j_r, k_r, reporting);

    [x_vector, fval, exitflag, output] = fminsearch(AnonFn, x_0, optimset('TolX',0.0001, 'TolFun', 0.01, 'PlotFcns', @optimplotfval));

    affine_matrix = MimImageCoordinateUtilities.CreateAffineTranslationMatrix(x_vector);

    transformed_matrix = PTKRegisterImageAffineUsingCoordinates(image_to_transform, reference_image, affine_matrix, i_o, j_o, k_o, i_r, j_r, k_r, '*linear', reporting);
end

function closeness = FnToMinimiseRigid(x_vector, image_to_transform, reference_image, i_o, j_o, k_o, i_r, j_r, k_r, reporting)
    affine_matrix = MimImageCoordinateUtilities.CreateAffineTranslationMatrix(x_vector);
    transformed_image = PTKRegisterImageAffineUsingCoordinates(image_to_transform, reference_image, affine_matrix, i_o, j_o, k_o, i_r, j_r, k_r, '*linear', reporting);
    closeness = ComputeCloseness(transformed_image, reference_image);
end

function closeness = ComputeCloseness(image1, image2)
    diff = (image1.RawImage - image2.RawImage).^2;
    closeness = sqrt(sum(diff(:))/numel(image1.RawImage));
end


function [m0, l0, a0, b0] = tapas_rdcm_get_prior(DCM)
% [m0, l0, a0, b0] = tapas_rdcm_get_prior(DCM)
%
% Returns prior parameters on model parameters (theta) and noise precision
% (tau) for the particular connectivity pattern
%
%   Input:
%   	DCM             - model structure
%
%   Output:
%       m0              - prior mean
%       l0              - prior covariance
%       a0              - prior shape parameter
%       b0              - prior rate parameter
%

% ----------------------------------------------------------------------
%
% Authors: Stefan Fraessle (stefanf@biomed.ee.ethz.ch), Ekaterina I. Lomakina
%
% Copyright (C) 2016-2022 Translational Neuromodeling Unit
%                         Institute for Biomedical Engineering
%                         University of Zurich & ETH Zurich
%
% This file is part of the TAPAS rDCM Toolbox, which is released under the
% terms of the GNU General Public License (GPL), version 3.0 or later. You
% can redistribute and/or modify the code under the terms of the GPL. For
% further see COPYING or <http://www.gnu.org/licenses/>.
%
% Please note that this toolbox is in an early stage of development. Changes
% are likely to occur in future releases.
%
% ----------------------------------------------------------------------

% get the number of regions and inputs
[nr, nu] = size(DCM.c);

% get the priors on model parameters
[pE,pC] = tapas_rdcm_spm_dcm_fmri_priors(DCM.a,DCM.b,DCM.c,DCM.d);

% set the prior mean of endogenous parameters to zero
pE.A = zeros(size(pE.A))+diag(diag(pE.A));

% specify the prior mean
m0 = [pE.A reshape(pE.B,nr,nr*nu) pE.C];

% prior precision
pC.A       = 1./pC.A;
pC.B       = 1./pC.B;
pC.C       = 1./pC.C;
pC.D       = 1./pC.D;
pC.transit = 1./pC.transit;
pC.decay   = 1./pC.decay;
pC.epsilon = 1./pC.epsilon;

% prior precision of baseline
if ( any(pC.C(:,end)) )
    pC.C(:,end) = 10^-8;
end

% prior precision
l0 = [pC.A reshape(pC.B,nr,nr*nu) pC.C];

% Setting priors on noise precision
a0 = 2;
b0 = 1;

end


function [fBValue, fStdDev, fMc, fAValue, nNumberQuakes] = calc_BandMc(mCatalog, nMinimumNumber, mcCalcMethod, fBinning, bConstrainMc, fMcMin, fMcMax, fMcAdd)
%  b-values (with stddev) and the magnitudes of completeness of a given catalog
%
  % [fBValue, fStdDev, fMc, fAValue, nNumberQuakes] = calc_BandMc(mCatalog, nMinimumNumber, mcCalcMethod, fBinning, bConstrainMc, fMcMin, fMcMax, fMcAdd)
% --------------------------------------------------------------------------------------------------------------------------------------------------------------
% Calculation of the b-values, its standard deviation and the magnitudes of completeness of a given catalog
%
% Input parameters:
%   mCatalog            Earthquake catalog
%   nMinimumNumber      Minimum number of earthquakes in the catalog for calculating the output values
%   mcCalcMethod        Method to determine the magnitude of completeness (see also: McMethods)
%   fBinning            Magnitude binning of the catalog (default 0.1)
%   bConstrainMc        Constrain Mc to [fMcMin, fMcMax] if set to 1 (default 0)
%   fMcMin              see bConstrainMc
%   fMcMax              see bConstrainMc
%   fMcAdd              Value to be added to Mc after potential constraining and prior to
%                       b-value computation for conservative computations
%
% Output parameters:
%   fBValue             b-value of the catalog with respect to magnitude of completeness
%   fStdDev             Standard deviation of the b-value (Shi & Bolt)
%   fMc                 Magnitude of completeness (Mc)
%   fAValue            a-value of the catalog
%   nNumberQuakes       Number of quakes used to compute the b-value (M > Mc)
%
% Copyright (C) 2003 by Danijel Schorlemmer
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the
% Free Software Foundation, Inc.,
% 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.

report_this_filefun();

% Magnitude binning
if ~exist('fBinning', 'var')
  fBinning = 0.1;
end

% Constrain magnitude of completeness
if ~exist('bConstrainMc', 'var')
  bConstrainMc = false;
end
if ~exist('fMcMin', 'var')
  fMcMin = 0;
end
if ~exist('fMcMax', 'var')
  fMcMax = 1;
end

% Conservative adding of a constant
if ~exist('fMcAdd', 'var')
  fMcAdd = 0;
end

% Init output variables
fBValue = nan;
fStdDev = nan;
fMc = nan;
fAValue = nan;

try
  % Determine magnitude of completeness
  fMc = calc_Mc(mCatalog, mcCalcMethod, fBinning);
  % Constrain magnitude of completeness
  if bConstrainMc
    if fMc < fMcMin
      fMc = fMcMin;
    elseif fMc > fMcMax
      fMc = fMcMax;
    end
  end
  % Conservative Mc
  fMc = fMc + fMcAdd;
  % Calculate the b-value of the learning period
  vSel_ = mCatalog.Magnitude >= fMc;
  mCatalog = mCatalog.subset(vSel_);
  nNumberQuakes = mCatalog.Count;
  if nNumberQuakes >= nMinimumNumber
    [fBValue, fStdDev, fAValue] =  calc_bmemag(mCatalog.Magnitude, fBinning);
  end
  if isempty(fMc)
    fMc = nan;
  end
catch
end


%CLASSC Convert classifier to normalized classifier (yielding confidences)
%
%  V = CLASSC(W)
%  V = W*CLASSC
%  D = CLASSC(A*W)
%  D = A*W*CLASSC
%  D = CLASSC(A,W)
%
% INPUT
%  W  Trained or untrained classifier
%  A  Dataset
%
% OUTPUT
%  V  Normalized classifier producing confidences instead of
%     densities or distances (after training if W is untrained)
%
% DESCRIPTION
% The trained or untrained classifier W may yield densities or unnormalised
% confidences. The latter holds for two-class discriminants like FISHERC
% and SVC as well as for neural networks. Such classifiers use or should
% use CNORMC to convert distances to confidences. In multi-class problems
% as well as in combining schemes they do not produce normalises
% confidences. These outcomes, like the density outcomes of classifiers
% liek QDC, LDC and PARZENC, can be converted by CLASSC into confidences:
% the sum of the outcomes will be one for every object.
%
% In case W is a one-dimensional mapping, it is converted into a two-class
% classifier, provided that during the construction a class label was
% supplied. If not, the mapping cannot be converted and an error is
% generated.
%
% CLASSC lists the outcomes on the screen in case no output argument is
% supplied. Also true and estimated labels are supplied.
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, CNORMC, LABELD

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

% $Id: classc.m,v 1.5 2010/02/23 15:21:54 duin Exp $

function w = classc(w,flag)

		if nargin < 2, flag = 0; end % flag forces non=combiner behavior avoiding recursion

	if (nargin == 0)

		% Untrained mapping.
		w = prmapping('classc','combiner',flag);

	elseif (ismapping(w))

		% If mapping is stacked or parallel, recurse over the individual
		% sub-mappings and call CLASSC for each of them.
		if ((isstacked(w)) | (isparallel(w))) & (flag == 0)
			v = cell(1,length(w.data));
			for j = 1:length(w.data)
				if ismapping(w.data{j}) % the parallel combiner may have nonmapping data
					v{j} = feval(mfilename,w.data{j});
				else
					v{j} = w.data{j};
				end
			end
			w = setdata(w,v);
			w = feval(mfilename,w,1);  % and here CLASSC is called for the combiner avoiding recursion
		else
			conv = get(w,'out_conv');
			if (conv < 1)
				% Set the "normalization" bit in the mapping's output conversion flag
				w = set(w,'out_conv',conv+2);
			end;
		end
	elseif (isdataset(w))
		if ismapping(flag)
			if nargout == 1
				w = feval(mfilename,w*flag);
			else
				feval(mfilename,w*flag);
				clear w;
			end
			return
		end
		w = w*normm;
		w = w*costm;
		if nargout == 0 % list outcomes on the screen
			ww = +w;
			ss = repmat('-',1,9*size(ww,2));
			fprintf('\n True   Estimated        Class \nLabels    Labels      Confidences\n');
			fprintf('------------------%s\n',ss);
			nlab = getnlab(w);
			[wmax,K] = max(ww,[],2);
			lablist = getlablist(w);
			if ~isempty(lablist) & ~ischar(lablist)
				nlab = lablist(nlab);
				K = lablist(K);
			end
			for j=1:size(ww,1)
				if (nlab(j) ~= K(j))
					fprintf(' %3.0f    ->%3.0f    ',nlab(j),K(j));
				else
					fprintf(' %3.0f      %3.0f    ',nlab(j),K(j));
				end
				fprintf('  %7.4f',ww(j,:));
				fprintf('\n');
			end
			lablist = getlablist(w);
			if ischar(lablist)
				fprintf('\n');
				for j=1:size(lablist,1)
					fprintf('   %i %s\n',j,lablist(j,:));
				end
			end

			clear w;
		end
	else
		error('input should be mapping or dataset');
	end

return


function M_hat = run_mc(params)
  M = params.M;
  Idx = params.Idx;
  warning('off','all');
  [numr,numc] = size(M);
  rank = 2;
  Known = find(Idx);
  data = M(Known);
  [U,S,V] = svp(Known,data,numr,numc,rank);
  M_hat = U*diag(S)*V';
  warning('on','all');
end


classdef Sigmoid < dagnn.ElementWise
  methods
    function outputs = forward(obj, inputs, params)
      outputs{1} = vl_nnsigmoid(inputs{1}) ;
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
      derInputs{1} = vl_nnsigmoid(inputs{1}, derOutputs{1}) ;
      derParams = {} ;
    end
  end
end


function prob_map_obj = atlas_get_probability_maps(obj, varargin)
% Given an atlas object, return probability maps or indicator maps based on parcels
%
% :Usage:
% ::
% prob_map_obj = atlas_get_probability_maps(obj)
%
% :Inputs:
%
%   **obj:**
%        an atlas-class object
%
% :Outputs:
%
%   **prob_map_obj:**
%        an fmri_data object containing the probability maps or indicators
%        as data
%
% Copyright 2018 Tor Wager

% Get maps if they exist
% -------------------------------------------------------------------------

if isempty(obj.probability_maps)

    obj.probability_maps = single(condf2indic(obj.dat, 'integers'));

    obj.probability_maps(isnan(obj.probability_maps)) = 0;

end

% Get maps and construct an object
% -------------------------------------------------------------------------

prob_map_obj = image_vector('dat', single(full(obj.probability_maps)), ...
    'volInfo', obj.volInfo, ...
    'removed_voxels', obj.removed_voxels, 'removed_images', obj.removed_images, ...
    'image_names', obj.image_names, 'noverbose');

prob_map_obj = fmri_data(prob_map_obj);

end % function


%clear all

%fname = 'D:\MatData\umeda data2\modified_motion01_03.mat';
%%load(fname)
%
%ydata = ydata(2:3,:,:);
%
%xdata(:,:,ind) = [];
%ydata(:,:,ind) = [];

%fsave = 'test_spike.mat';
%
%load(fsave)

ypred = predict_output(xtest, Model, parm);
ydev  = predict_variance(xtest, Model, parm);

ymin = min(ydev(:));
ydev = ydev - ymin;

yerr = abs(ypred - ytest);

[err,rcor] = mean_sq_error(yerr,ydev)

NY = 2; NX = 1;
nfig = 1;
subplot(NY,NX,nfig)
%plot(yerr(1,:,1))
plot(yerr(1,:,1)/max(yerr(1,:,1)))
hold on

%nfig = nfig+1;
subplot(NY,NX,nfig)
%plot(ydev(1,:,1),'-r')
hold on
plot(ydev(1,:,1)/max(ydev(1,:,1)),'-r')

return

[M,T,Ntr] = size(xdata);
[N,T,Ntr] = size(ydata);

x = reshape(xdata,[M,T*Ntr]);
y = reshape(ydata,[N,T*Ntr]);

NY = 5; NX = 2;
nfig = 0;

for n=1:N
	nfig = nfig+1;
	if nfig > NX*NY, figure; nfig=1; end
	subplot(NY,NX,nfig)
	plot(y(n,:))
end

figure

NY = 6; NX = 2;

for n=1:M
	subplot(NY,NX,n)
	plot(x(n,:))
end

%save(fsave,'xdata','ydata')


randn('seed', 1e6);
rand('seed', 1e6);
if ~exist('mappingKern', 'var'), mappingKern = 'rbfardjit'; end
if ~exist('indPoints', 'var'), indPoints = 20; end
if ~exist('latentDim', 'var'), latentDim = 10; end
% Define a temporal model
if ~exist('dynamicsConstrainType', 'var'), dynamicsConstrainType = {'time'}; end
if ~exist('initVardistIters', 'var'), initVardistIters = 500; end
if ~exist('itNo', 'var'), itNo = [50 50]; end
if ~exist('dynamicKern', 'var'), dynamicKern = {'matern32','white','bias'}; end

%%
% load data
[Y, lbls] = lvmLoadData('brendan');
Y = Y(801:880,:);
t = linspace(1,2*pi, size(Y,1))';

h = 20;
w = 28;
scrsz = get(0,'ScreenSize');
figure('Position',[0.2*scrsz(3) 0.2*scrsz(4) 20*5 28*5])
for i=1:size(Y,1)
    imagesc(reshape(Y(i,:), h, w)'); colormap('gray');
    pause(0.05);
end

%% Split between training and test blocks
mask = [];
lastTrPts = 5;
r=1; % start with tr. set
while length(mask)<size(Y,1)-lastTrPts %The last lastTrPts will be from YTr necessarily
    blockSize = randperm(8);
    blockSize = blockSize(1);
    pts = min(blockSize, size(Y,1)-lastTrPts - length(mask));
    if r
        mask = [mask ones(1,pts)];
    else
        mask = [mask zeros(1,pts)];
    end
    r = ~r; % alternate between tr. and test set
end
mask = [mask ones(1,lastTrPts)];
indTr = find(mask);
indTs = find(~mask);
if sum(sort([indTr indTs]) - (1:size(Y,1)))
    error('Something went wrong in the dataset splitting...');
end
Nstar = length(indTs);

Ytr = Y(indTr,:); t_tr = t(indTr, :);
Yts = Y(indTs,:); t_ts = t(indTs, :);

% Additionally define some pixels to be observed for the test set
cutPoint=round(h/2)+1;
mask = zeros(h,1);
mask(cutPoint) = 1;
mask = [ones(1,cutPoint) zeros(1,h-cutPoint)];
mask=repmat(mask, 1,w);
indexMissing = find(mask);

YtsOriginal = Yts;
Yts(:, indexMissing) = NaN;
% Play the test set fully observed (left) and partially observed (right)
scrsz = get(0,'ScreenSize');
figure('Position',[0.2*scrsz(3) 0.2*scrsz(4) 20*15 28*5])
for i=1:size(Yts,1)
    subplot(1,2,1)
    imagesc(reshape(YtsOriginal(i,:), h, w)'); colormap('gray');
    subplot(1,2,2);
    imagesc(reshape(Yts(i,:), h, w)'); colormap('gray');
    pause(0.05);
end
%% Train models

% Options for the model
vargplvm_init; % Returns a configuration structure 'globalOpt'
options = vargplvmOptions('dtcvar');
options.kern = mappingKern;
options.numActive = indPoints;
options.optimiser = 'scg2';
options.latentDim = latentDim;
options.initSNR = 100;
if ~isempty('dynamicsConstrainType')
    % Temporal model (VGPDS)
    optionsDyn.type = 'vargpTime';
    optionsDyn.t=t;
    if ~isstruct(dynamicKern)
        optionsDyn.kern = kernCreate(t, dynamicKern);
    else
        optionsDyn.kern = dynamicKern;
    end
    % Create and optimise the model
    [~, ~, ~, ~, modelInitVardist] = vargplvmEmbed2(Ytr, latentDim, options, initVardistIters, 0, true, optionsDyn);
     model = vargplvmOptimiseModel(modelInitVardist, true, true, {0,itNo}, true);
else
    % Non-dynamical model (Bayesian GP-LVM)
    % Create and optimise the model
    [~, ~, ~, ~, modelInitVardist] = vargplvmEmbed2(Ytr, latentDim, options, initVardistIters, 0, true);
    model = vargplvmOptimiseModel(modelInitVardist, true, true, {0,itNo}, true);
end

%%

fprintf(1, '# Predicting only with the test time points...\n')
[Testmeans2 Testcovars2] = vargplvmPredictPoint(model.dynamics, t_ts);
Varmu2 = vargplvmPosteriorMeanVar(model, Testmeans2, Testcovars2);
% Mean absolute error per pixel
errorOnlyTimes = mean(abs(Varmu2(:) - YtsOriginal(:)));

%%
% Play the test set fully observed (left) and predicted (right)
scrsz = get(0,'ScreenSize');
figure('Position',[0.2*scrsz(3) 0.2*scrsz(4) 20*15 28*5])
t_ind = 1:size(Y,1);
t_ind = t_ind(indTs);
for i=1:size(Yts,1)
    subplot(1,2,1)
    imagesc(reshape(YtsOriginal(i,:), h, w)'); colormap('gray'); title(num2str(t_ind(i)))
    subplot(1,2,2);
    imagesc(reshape(Varmu2(i,:), h, w)'); colormap('gray');
    pause;
end


function [fdMc, fProbability, fAICc] = calc_llhd_dMc2(mCat1, mCat2)
% function [fdMc, fProbability, fAICc] = calc_llhd_dMc2(mCat1, mCat2);
% ----------------------------------------------------------------------------------------------
% Calculate log-likelihood estimation of a magnitude of completness change between the two periods
%
% Incoming variable
% mCat1 : EQ catalog period 1 (Catalog to be modified)
% mCat2 : EQ catalog period 2 (Observed catalog)
%
% Outgoing variable
% fdMc         : Change in the magnitude of completeness
% fProbability : log-likelihood probabilty
% fAICc         : Corrected Akaike Information Criterion
%
% Author: J. Woessner, woessner@seismo.ifg,.ethz.ch
% last update: 21.10.03

fBinning = 0.1;
nSample = 100;
nMethod = 6;

% Determine exact time period
fPeriod1 = max(mCat1(:,3)) - min(mCat1(:,3));
fPeriod2 = max(mCat2(:,3)) - min(mCat2(:,3));

% Initialize values
fMinMag = min([min(mCat1(:,6)) min(mCat2(:,6))]);
fMaxMag = max([max(mCat1(:,6)) max(mCat2(:,6))]);

try
%% Calculate model for best fitting Mc, both periods
[mResult1, fMls1, fMc1, fMu1, fSigma1, mDataPredBest1, vPredBest1, fBValue1, fAvalue1] = calc_McCdfnormal(mCat1, fBinning);
[mResult, fMls, fMc2, fMu, fSigma, mDataPredBest, vPredBest, fBValue2, fAvalue2] = calc_McCdfnormal(mCat2, fBinning);

% [fMc1, fStd_Mc1, fBvalue1, fStd_B1, fAvalue1, fStd_A1, vMc1, mBvalue1] = calc_McBboot(mCat1, fBinning, nSample, nMethod)
% [fMc2, fStd_Mc2, fBvalue2, fStd_B2, fAvalue2, fStd_A2, vMc2, mBvalue2] = calc_McBboot(mCat2, fBinning, nSample, nMethod)

% Mc difference
fdMc = fMc2-fMc1;

% Select part of catalog
vSel = mCat1(:,6) >= fMc2-fBinning/2;
mCat1tmp=mCat1(vSel,:);

% Create Model distribution
vMagnitudes = [fMc2:0.1:floor(max(mCat2(:,6)))+0.1];
% Productuvity for fMc2
nNumberEvents = length(mCat1tmp(:,1));
vNumbers = 10.^(log10(nNumberEvents) - fBValue1*(vMagnitudes-fMc2));
%vNumbers = 10.^(fAvalue1- fBValue1*(vMagnitudes-fMc2));
vNumbers = round(vNumbers);
vNCumFMD = round(-diff(vNumbers));
% Calculate synthetic data below Mc
fMinMag = min(mCat2(:,6));
vMagstep = fMinMag:0.1:fMc2-0.1;
vProb = normcdf(vMagstep,fMu1, fSigma1);
vProb = vProb';
vMagstep = vMagstep';

% Calculate number of EQs in bins
fN_Mc = vNCumFMD(1,1);
vN = round(vProb(:,1)*fN_Mc);
%mNonCumModel = [vN vMagstep; vNCumFMD' vBin'];
mNonCumModel = [vN vMagstep; vNCumFMD' vMagnitudes(:,1:end-1)'];

% vMags = mDataPredBest(:,2)';
% FMD to be modeled (second period)
[vObsFMD,vBin2] = hist(mCat2(:,6),roundn(min(mCat2(:,6)),-1):0.1:floor(max(mCat2(:,6))));

% Select bins to calculate loglikelihood
vSel = (mNonCumModel(:,2) < min(vBin2) | mNonCumModel(:,2) > max(vBin2));
vPredFMD = mNonCumModel(~vSel,:);
% Normalize
vObsFMD = ceil(vObsFMD./fPeriod2);
vPredFMD(:,1) = vPredFMD(:,1)./fPeriod1;

if length(vObsFMD') ~= length(vPredFMD(:,1))
    disp('warning')
end
% Calculate the likelihood
vProb_ = calc_log10poisspdf2(vObsFMD', vPredFMD(:,1));
% Sum the probabilities
fProbability = (-1) * sum(vProb_);

nDegFree = 1; % degree of freedom
n_samples = length(mCat2(:,6));
%% Corrected Akaike Information Criterion (AICc)
fAICc = -2*(-fProbability)+2*nDegFree+2*nDegFree*(nDegFree+1)/(n_samples-nDegFree-1);

catch
    fdMc = nan;
    fProbability = nan;
    fAICc = nan;
end
% figure_w_normalized_uicontrolunits(30)
% %plot(mNonCumModel(:,2),mNonCumModel(:,1),'r*',vBin2,vObsFMD,'o')
% histogram(mCat1(:,6),fMinMag:0.1:fMaxMag);
% hold on
% histogram(mCat2(:,6),fMinMag:0.1:fMaxMag)
% h = findobj(gca,'Type','patch');
% set(h,'FaceColor','r','EdgeColor','w')
% plot(vPredFMD(:,2),vPredFMD(:,1),'g*',vBin2,vObsFMD,'o')


% Sigmoid Belief Hidden Markov Decision Tree    (Jordan/Gharhamani 1996)
%
clear all;
%clc;
rand('state',0); randn('state',0);
X = 1; Q1 = 2; Q2 = 3; Y = 4;
% intra time-slice graph
intra=zeros(4);
intra(X,[Q1 Q2 Y])=1;
intra(Q1,[Q2 Y])=1;
intra(Q2, Y)=1;
% inter time-slice graph
inter=zeros(4);
inter(Q1,Q1)=1;
inter(Q2,Q2)=1;

ns = [1 2 3 1];
dnodes = [2 3];
eclass1 = [1 2 3 4];
eclass2 = [1 5 6 4];
bnet = mk_dbn(intra, inter, ns, dnodes, eclass1, eclass2);

bnet.CPD{1} = root_CPD(bnet, 1);
% =========================================
bnet.CPD{2} = softmax_CPD(bnet, 2);
bnet.CPD{3} = softmax_CPD(bnet, 3, 'discrete', [2]);
bnet.CPD{5} = softmax_CPD(bnet, 6);
bnet.CPD{6} = softmax_CPD(bnet, 7, 'discrete', [3 6]);
% =========================================
bnet.CPD{4} = gaussian_CPD(bnet, 4);

% make some data
T=20;
cases = cell(4, T);
cases(1,:)=num2cell(round(rand(1,T)*2)+1);
%cases(2,:)=num2cell(round(rand(1,T))+1);
%cases(3,:)=num2cell(round(rand(1,T)*2)+1);
cases(4,:)=num2cell(rand(1,T));

engine = bk_inf_engine(bnet, 'exact', [1 2 3 4]);

% log lik before learning
[engine, loglik] = enter_evidence(engine, cases);

% do learning
ev=cell(1,1);
ev{1}=cases;
[bnet2, LL2] = learn_params_dbn_em(engine, ev, 10);


function perm=field3d(Nx,Ny,Nz,k_avg,V_dp,clx,cly,clz)
% Written by Ali A. Eftekhari
% See the license file
% Does not work! Needs lots of improvements
Lx=1.0; % domain length
x=linspace(-Lx/2.0,Lx/2.0,Nx);
y=linspace(-Lx/2.0,Lx/2.0,Ny);
z=linspace(-Lx/2.0,Lx/2.0,Nz);
[X,Y,Z]=ndgrid(x,y,z);
s=-log(1-V_dp); % standard deviation
mu=log(k_avg)-s*s/2.0; % mean for a log-random field
Z=s*randn(Nx,Ny,Nz); % normal distribution
F = exp(-(X.^2/(clx*clx/2.0)+Y.^2/(cly*cly/2.0)+Z.^2/(clz*clz/2.0)));
% Gaussian filter
f =2.0/sqrt(pi)*Lx/(Nx*Ny*Nz)^(1/3)/(clx*cly*clz)^(1/3).*ifftn(fft2(Z).*fftn(F)); % another filter
perm=exp(mu+real(f)); % perm field


function [geo] = GetGeometryfromGUI(root, tip, wing, cruise, engine, hplot, output, airfoildata)
% Get parameters from the GUI and create geometry structure "geo"
% Gets the user defined parameters from the GUI
% Performs limited error checking and sets boolean to tell ExecCalc to continue or not

%Clear clock
set(output.calctime,'String','----');

%Set wing geometry from values from GUI
geo.t0 = clock; %Start time
geo.b = str2num(get(wing.span,'String'))/3.281;  %Convert to meters
geo.c_r = str2num(get(root.chord,'String'))/3.281;  %Convert to meters
geo.taper = str2num(get(tip.chord,'String'))/str2num(get(root.chord,'String'));
geo.i_r = str2num(get(root.angle,'String'))*pi/180;   %Convert to radians
geo.twist = (str2num(get(tip.angle,'String'))-str2num(get(root.angle,'String')))*pi/180;  %Convert to radians
geo.dih = str2num(get(wing.dihedral,'String'))*pi/180; %Convert to radians
geo.root = airfoildata{1}{get(root.airfoil,'Value')};
geo.tip = airfoildata{1}{get(tip.airfoil,'Value')};
geo.alpha = str2num(get(wing.AOA,'String'))*pi/180; %Convert to radians
geo.V = str2num(get(cruise.velocity,'String'))*0.5144; %Convert knots to m/s
[geo.density, geo.a geo.visc] = StandardAtmosphere(get(cruise.altitude,'String'));
geo.M = geo.V/geo.a;
geo.S_Sref = str2num(get(cruise.wettedarea,'String'));  %Dimensionless
geo.cf = str2num(get(cruise.skinfriction,'String'));  %Dimensionless
geo.ns = str2num(get(wing.ns,'String'));
geo.nc = str2num(get(wing.nc,'String'));
geo.sweep = str2num(get(wing.sweep,'String'))*pi/180;  %Convert to radians
geo.S = 0.5*(geo.c_r + geo.c_r*geo.taper)*geo.b;  %Square meters
geo.c_av = 0.5*(geo.c_r + geo.c_r*geo.taper);  %Meters
geo.Re_r = geo.V*geo.density*geo.c_r/geo.visc; %Reynolds number at root
geo.Re_t = geo.V*geo.density*geo.c_r*geo.taper/geo.visc; %Reynolds number at root
geo.rootindex = get(root.airfoil,'Value');      %index to selected root airfoil
geo.tipindex = get(tip.airfoil,'Value');         %index to selected tip airfoil
geo.propefficiency = str2num(get(engine.propeffic,'String'));  %from Anderson, Aircraft Performance and Design
geo.propSFC = str2num(get(engine.SFC,'String'))*1.657e-6; % convert lb fuel/hp-hr to m^-1 Wikipedia (Specific Fuel Consumption)
geo.jetTSFC = str2num(get(engine.TSFC,'String'))/3600; %convert to lb fuel per lb-s thrust; Anderson, Aircraft Performance and Design pg. 299
geo.emptyweight = str2num(get(cruise.weight,'String'))*4.44;%Convert weight from lbf to N
geo.fueldens = 0.840*1e3; %(kg/m^3) Density of Jet A-1 at 15deg C (Wikipedia)
geo.wingfuel = str2num(get(engine.wingfuel,'String'))/100;  %Percent of wing dedicated to carrying fuel
geo.withinconstraints = 1; % Set boolean flag stating that all geometry is within constraints

%Set the selected type of engine
if get(engine.panel,'SelectedObject')==engine.prop
    geo.engine = 'prop';
elseif get(engine.panel,'SelectedObject')==engine.jet
    geo.engine = 'jet';
else
    geo.engine = 0;
end

%Set initial output
set(wing.taper,'String',num2str(geo.taper));
set(wing.twist,'String',num2str(geo.twist));
set(root.Re,'String',num2str(round(geo.Re_r/1e3)/1e3));
set(tip.Re,'String',num2str(round(geo.Re_t/1e3)/1e3));
set(cruise.density,'String',num2str(round(geo.density*1000)/1000));
set(cruise.viscosity,'String',num2str(round(geo.visc*10000000)/10000000));
set(cruise.mach,'String',num2str(round(geo.M*100)/100));

%Limited error checking
if geo.sweep < 80*pi/180  %If sweep < 80 deg
    set(wing.sweep,'ForegroundColor','black')
else
    set(wing.sweep,'ForegroundColor','red')
    ZeroOutput(output)
    geo.withinconstraints = 0; % User defined geometry is not within constraints
end
if geo.dih < 80*pi/180  %If dihedral < 80 deg
    set(wing.dihedral,'ForegroundColor','black')
else
    set(wing.dihedral,'ForegroundColor','red')
    ZeroOutput(output)
    geo.withinconstraints = 0; % User defined geometry is not within constraints
end
if geo.M < 0.65  %If M < 0.65
    set(cruise.mach,'ForegroundColor','black')
else
    set(cruise.mach,'ForegroundColor','red')
    ZeroOutput(output)
    geo.withinconstraints = 0; % User defined geometry is not within constraints
end


function f = logical(f)
%LOGICAL   CHEBFUN logical.
%   LOGICAL(F) returns a CHEBFUN which evaluates to one at all points where F is
%   non-zero and zero otherwise.

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

% Add breaks at the roots (since these will take the value 0 in the output).
f = addBreaksAtRoots(f);

% Loop over the FUNs:
for k = 1:numel(f.funs)
    f.funs{k} = logical(f.funs{k});
end

% pointValues:
tol = vscale(f)*eps;
f.pointValues = abs(f.pointValues) > tol;

end


function areaV = calculate_structure_cross_sectional_area(structIndV,planC)
% function areaV = calculate_structure_cross_sectional_area(structIndV,planC)
%
% APA, 08/29/2012

if ~exist('planC','var')
    global planC
end
indexS = planC{end};

if ischar(structIndV)
    structIndV = {structIndV};
end

structureNamesC = {planC{indexS.structures}.structureName};
for i = 1:length(structIndV)
    if isnumeric(structIndV(i))
        structNum = structIndV(i);
    else
        structureName = structIndV{i};
        structNum = getMatchingIndex(structureName,structureNamesC,'regex');
        if length(structNum) > 1
            structNum = structNum(1);
        end
    end

    if isempty(structNum) || length(structNum) > 1
        areaV(i) = NaN;
        continue
    end

    [rasterSegments, planC, isError] = getRasterSegments(structNum,planC);
    if isempty(rasterSegments)
        areaV(i) = NaN;
        continue;
    end
    scanNum = getStructureAssociatedScan(structNum,planC);
    [~, uniqueSlices] = rasterToMask(rasterSegments, scanNum, planC);
    middleSlcNum = round(median(uniqueSlices));

    areaVal = 0;
    for segNum = 1:length(planC{indexS.structures}(structNum).contour(middleSlcNum).segments)
        areaVal = areaVal + polyarea(planC{indexS.structures}(structNum).contour(middleSlcNum).segments(segNum).points(:,1),planC{indexS.structures}(structNum).contour(middleSlcNum).segments(segNum).points(:,2));
    end

    areaV(i) = areaVal;
end


% function [Tk,dTk] = interp2DlinearP(TD,Omega,X,doderivative)
%
% (c) NP, Safir, Luebeck 2006
% this is a function for linear interpolation
% input:
%     - TD      :   the data that has to be interpolated (a 3D matlab
%     matrix)
%     - Omega   :   the image domain
%     - X       :   the interpolaiton points (a long vector; see getGrid
%     for example)
%     - doderivative : flag for computing the derivative
%
% output:
%     - Tk      : the interpolated function values (same format as X)
%     - dTk     : the derivative of the linear interpolated function with
%     respect to X (computed when doderivative is 1)
%
function [Tk,dTk] = interp2DlinearP(TD,Omega,X,periode,doderivative);

Tk = [];
dTk = [];

% if no input is given a testcase will be created and then
% the derivative test will be started
if nargin == 0
  fprintf('%s\n',mfilename);
  Omega = [1, 1];
  %TD = [zeros(2,7); ones(3,1)*[0 0 0.5 1 1.5 0 0.5];zeros(2,7)];
  TD = [0 1.5;  0 1.5];
  TD = TD';
  periode = 2;
%   figure(1);clf;
%   image(TD./periode.*256); colormap(hsv(256)); axis image;

  m = [100,100];
  X = getGrid(Omega,m);
  %    m = [1,1];
  %    X = [0.5; 0.5];
interp2DlinearP(TD,Omega,X,periode,0);

%    viewImage(Tk./periode*256,Omega,m,'fig',2);
%     figure(3); clf;
%     surf(reshape(Tk,m)');
  return;
end;
% if nargout == 0 start a derivative test
if nargout == 0
  % derivative test
  fprintf('derivative(%s)\n',mfilename);
  [fc,dfc] = feval(mfilename,TD,Omega,X,periode,1);
  v   = randn(size(X));
  %   %save v1 v
  %   load v1
  %   e = zeros(size(v));
  %   e(1) = v(1);
  %   v = e;
  %   v
  dfv = dfc*v;
  hh  = logspace(0,-10,11);
  fprintf('%12s %12s %12s \n','h','|fc-ft|','|fc+vdfc-ft|');
  for j=1:length(hh),
    Xt = X + hh(j)*v;
    ft = feval(mfilename,TD,Omega,Xt,periode,0);
    n1 = norm(fc(:)-ft(:));
    n2 = norm(fc(:)+hh(j)*dfv-ft(:));
    fprintf('%12.4e %12.4e %12.4e\n',hh(j),n1,n2);
  end;
  return;
end;

% if doderivative is not explicitly given
% a derivative is computed if two output arguments are given
if isempty(doderivative), doderivative = (nargout > 0); end;
% n is the number of interpolation points
% mD is the size of the interpolation dat
n = length(X)/2;
mD = size(TD);
periode2 = periode/2;
% zeropadding to avoid bad cases
% so we do an embedding of TD into a field of zeros
offset = 1;
To = zeros(size(TD)+2*offset);
To(offset+(1:size(TD,1)),offset+(1:size(TD,2))) = TD;

% get pixelsize, pay attention to the change of order from m to mD
hD = Omega./mD([2,1]);
% for easier reading
hD1 = hD(1);
hD2 = hD(2);
% for easier reading

% alloc memory for output

% alloc memory for output
Tk = zeros(n,1);
if doderivative, dTk = zeros(n,2); end;

X1 = X(1:n); X2 = X(n+(1:n));

% transform grid X to integer grid
X1 = (1/hD1)*X1 + 0.5;
X2 = (1/hD2)*X2 + 0.5;

% so if we have no good points, we have to format the
% derivative for numerical reasons

G = find( 0 < X1 & X1 < mD(2) + 1 & ...
  0 < X2 & X2 < mD(1) + 1    );

if isempty(G),
  dTk = sparse(n,2*n);
  return;
end;

% dj is the size we have to go to get the
% neigbhour in j direction
d1 = size(To,1);
d2 = -1;

% now we get into more detail
% Kj storages the noninteger part of Xj
K1 = floor(X1(G)); K2 = floor(X2(G));
Xi1 = X1(G) - K1;  Xi2 = X2(G) - K2;

K = offset + (offset + K1 - 1)*d1 + (mD(1) - K2 +1);

% compute Tk
% here now something new
dTs1 = To(K+d1)    - To(K);
dTs2 = To(K+d1+d2) - To(K+d2);

pO1 = zeros(size(K));
pO1(dTs1 > periode2)  = -periode;
pO1(dTs1 < -periode2) =  periode;

pO2 = zeros(size(K));
pO2(dTs2 > periode2)  = -periode;
pO2(dTs2 < -periode2) =  periode;

Tk1 = zeros(size(G));
Tk2 = zeros(size(G));

Tk1 =  To(K) .* (1-Xi1)  +  (To(K+d1) + pO1) .* Xi1;
Tk1(Tk1<0)        = Tk1(Tk1<0)        + periode;
Tk1(Tk1>=periode) = Tk1(Tk1>=periode) - periode;

Tk2 =  To(K+d2) .* (1-Xi1)  +  (To(K+d1+d2) + pO2) .* Xi1;
Tk2(Tk2<0)        = Tk2(Tk2<0)        + periode;
Tk2(Tk2>=periode) = Tk2(Tk2>=periode) - periode;

dTs3 = Tk2 - Tk1;
pO3 = zeros(size(K));
pO3(dTs3 > periode2)  = -periode;
pO3(dTs3 < -periode2) =  periode;

%keyboard;
Tk(G) = Tk1 .* (1-Xi2)  +  (Tk2 + pO3) .* Xi2;

Tk(Tk<0)        = Tk(Tk<0)        + periode;
Tk(Tk>=periode) = Tk(Tk>=periode) - periode;

% keyboard

% if derivative is needed compute it
% next step: the derivative ???

if doderivative,
  dTk(G,1) = (To(K+d1) + pO1 - To(K)   ) .* (1-Xi2) ...
    + (To(K+d1+d2) + pO2 - To(K+d2)).* (Xi2);
  dTk(G,2) = Tk2 + pO3 - Tk1;
end;
if doderivative,
  dTk = spdiags([dTk(:,1)/hD1, dTk(:,2)/hD2],[0,n],n,2*n);
end;

return;