%%%C-Type implementation for imu interpolation for iphone data
%oper=new output period
%cdef= col definitions [total, Time , x , y, z]
function [ini_time]=interp_imu(INFILES, OUTFILE, cdef, oper)

% fclose all;
% clear;
% DIRNAME='C:\Documents and Settings\Yigiter\Desktop\data\CalibTest1\';
% INFILES=[[DIRNAME '07-16-00-12-acclog.bin ']; [DIRNAME '07-16-00-12-gyrolog.bin']];
% OUTFILE=[DIRNAME 'imu.bin'];
% cdef=[5 2 3 4 5;5 2 3 4 5];
% oper=2;

%i/o files
nfiles=size(cdef,1);
ndat=size(cdef,2)-1;

infids=zeros(nfiles,1);
for (in=1:nfiles)
    infids(in,1)=fopen(INFILES(in,:),'rb');
end
ofid=fopen(OUTFILE,'wb');

%first data from all files
data=zeros(ndat,nfiles);
for in=1:nfiles
    vr_a=fread(infids(in),[cdef(in,1) 1],'double');
    data(:,in)=vr_a(cdef(in,2:end),1);
end

%Latest data
ct=data(1,1);
for in=2:nfiles
    if ct<data(1,in)
        ct=data(1,in);
    end
end

%Read all files till they past the latest
flag=ones(nfiles,1);
while (sum(flag,1))
    for in=1:nfiles
        if (data(1,in)<ct)
            vr_a=fread(infids(in),[cdef(in,1) 1],'double');
            data(:,in)=vr_a(cdef(in,2:end),1);
        else
            flag(in)=0;
        end
    end
end

ini_time=ct;

%%Write the first data (useless)
oin=0;
vr_a=reshape(data(2:end,:), (ndat-1)*nfiles, 1);
fwrite(ofid, [oin;vr_a],'double');

%Start the main part
ipt=ones(nfiles,1)*ct;
idt=zeros(nfiles,1);
data_inc=zeros(ndat,nfiles);
iflag=zeros(nfiles,1);
fflag=zeros(nfiles,1);

while (~sum(fflag,1))
    %%process all files till they exceed the next sampling point
    for in=1:nfiles
        if (~iflag(in))
            dt=data(1,in)-ipt(in);
            if (idt(in)+dt<=oper)
                data_inc(:,in)=data_inc(:,in)+dt*data(:,in);
                idt(in)=idt(in)+dt;

                ipt(in)=data(1,in);
                vr_a=fread(infids(in),[cdef(in,1) 1],'double');
                if (isempty(vr_a))
                    break;  %Why does not feof() work properly! whyyyyyyy
                end
                data(:,in)=vr_a(cdef(in,2:end),1);
            else
                data_inc(:,in)=data_inc(:,in)+(oper-idt(in))*data(:,in);
                ipt(in)=ipt(in)+(oper-idt(in));
                iflag(in)=1;
            end
        end
    end

    %Check if one the files reaches the feof
    for in=1:nfiles
        fflag(in)=feof(infids(in));
    end

    %%In all files pass the next sampling point, write the results
    if sum(iflag,1)==nfiles
        %%Write data
        oin=oin+1;
        vr_a=reshape(data_inc(2:end,:), (ndat-1)*nfiles, 1)/oper;
        fwrite(ofid, [oin;vr_a],'double');

        %Prepare for the next cycle
        idt=zeros(nfiles,1);
        data_inc=zeros(ndat,nfiles);
        iflag=zeros(nfiles,1);
    end
end

%%Close all the files
for in=1:nfiles
    fclose(infids(in));
end
fclose(ofid);

return;


function VisualizeHogFeatures(bdir,fly,fnum,varargin)
% function VisualizeHogFeatures(expdir,fly,fnum,'jabfile',jabfile)

[moviename,trxfilename,params,jabfile] = myparse(varargin,...
  'moviename','movie.ufmf','trxfilename','trx.mat','params',getSTParams,...
  'jabfile','');

if ~isempty(jabfile)
   J = load(jabfile,'-mat');
   moviename = J.x.file.moviefilename;
   trxfilename = J.x.file.trxfilename;
   params = J.x.stInfo;
end

moviename = fullfile(bdir,moviename);
trackfilename = fullfile(bdir,trxfilename);

fname = 'hf';
%% params
psize = params.psize;
nbins = params.nbins;
npatches_x = params.npatches_x;
npatches_y = params.npatches_y;
patchsz_x = psize*npatches_x;
patchsz_y = psize*npatches_y;
wd = params.wd;
scale = params.scale;

%% compute the bins

% this seems to be what the centers correspond to
bincenters = linspace(0,pi,nbins+1);
bincenters = bincenters(1:nbins);
dt = mean(diff(bincenters));
binedges = [bincenters(1)-dt/2,(bincenters(1:end-1)+bincenters(2:end))/2,bincenters(end)+dt/2];

%%

tracks = load(trackfilename);
tracks = tracks.trx;

[readfcn,nframes,fid,headerinfo] = get_readframe_fcn(moviename);
im1 = readfcn(fnum);

trackndx = fnum - tracks(fly).firstframe + 1;
% locy = round(tracks(fly).y(trackndx));
% locx = round(tracks(fly).x(trackndx));
% im1 = extractPatch(im1,...
%   locy,locx,tracks(fly).theta(trackndx),patchsz);
locy = tracks(fly).y(trackndx);
locx = tracks(fly).x(trackndx);
im1 = CropImAroundTrx(im1,...
  locx,locy,tracks(fly).theta(trackndx)-pi/2,(patchsz_x-1)/2,(patchsz_y-1)/2);

H = zeros(npatches_y,npatches_x,nbins);
firstframe = tracks(fly).firstframe;
parfor yy = 1:npatches_y
  for xx = 1:npatches_x
    for oo = 1:nbins
      pfname = fullfile(bdir,'perframe',sprintf('st_%s_%02d_%02d_%d.mat',fname,yy,xx,oo));
      q = load(pfname);
      trackndx = fnum - firstframe + 1;
      H(yy,xx,oo) = q.data{fly}(trackndx);

    end
  end
end

% plot
hfig = figure();%'Visible','off');
clf;
hax = axes('Position',[0,0,1,1]);
set(hfig,'Units','pixels','Position',get(0,'ScreenSize'));

im1curr = im1;

him = imshow(imresize(im1curr,scale));
axis image;
truesize;
colormap gray;
hold on;
axis off;

colors = hsv(nbins);
colors = colors([ (end/2+1):end 1:end/2],:);

[nr,nc,~] = size(im1);
% maxv2 = max(H(:));
maxv2 = 0.03;

hogpatch = [wd wd -wd -wd wd;-psize psize psize -psize -psize]/2;
h = [];
for xi = 1:ceil(nc/psize),
  cx = (psize/2 + (xi-1)*psize)*scale+ 1 ;
  if cx+psize/2 > nc*scale,
    break;
  end
  for yi = 1:ceil(nr/psize),
    cy = (psize/2 + (yi-1)*psize)*scale+ 1 ;
    if cy+psize/2 > nr*scale,
      break;
    end

    for bini = 1:nbins,
      tmp = bincenters(bini);
      curpatch = [cos(tmp) -sin(tmp); sin(tmp) cos(tmp)]*hogpatch;
      xcurr = cx + curpatch(1,:)*scale;
      ycurr = cy + curpatch(2,:)*scale;
      h(yi,xi,bini) = patch(xcurr,ycurr,colors(bini,:),'LineStyle','none','FaceAlpha',min(1,H(yi,xi,bini)/maxv2));
    end

  end
end
truesize(hfig);
% im = getframe(hax);


function [x, infos] = mu_conv_nmf(V, rank, t, in_options)
% Multiplicative update (MU) based convolutive non-negative matrix factorization (MU-Conv-NMF).
%
% The problem of interest is defined as
%
%
% Given a non-negative matrix V, factorized non-negative matrices {W, H} are calculated.
%
%
% Inputs:
%       matrix      V
%       rank        rank
%
% Output:
%       w           solution of w
%       infos       information
%
% References:
%
%
% This file is part of NMFLibrary
%
% This file has been ported by H.Kasai from
% convNMF_MM1.m and convNMF_MM2.m at https://github.com/lyn202206/ADMM-Convolutive-NMF.
%
% Ported by H.Kasai on June 29, 2022
%
% Change log:
%
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.metric_type = 'beta-div';
    local_options.d_beta = 2;
    local_options.alg = 'type1';

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize factors
    init_options = options;
    if ~isfield(options, 'x_init')
        W = zeros(m, rank, t);
        for i = 1 : t
            [init_factors, ~] = generate_init_factors(V, rank, init_options);
            W(:, :, i) = init_factors.W;
        end
        H = init_factors.H;
    else
        W = init_options.x_init.W;
        H = init_options.x_init.H;
    end

    % initialize
    epoch = 0;
    grad_calc_count = 0;

    options = check_divergence(options);
    sub_mode = sprintf('beta=%.1f', options.d_beta);
    if ~strcmp(options.metric_type, 'beta-div')
        sub_mode = options.metric_type;
    end
    method_name = sprintf('MU-Conv (%s)', sub_mode);

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    % initialize for this algorithm
    V_hat = zeros(m, n);
    for i = 0 : t-1
        V_hat = V_hat + W(:, :, i+1) * shift_t(H, i);
    end

    % store initial info
    clear infos;

    [Wcon, Hcon] = reconstruct_wh(W, H, t);
    [infos, f_val, optgap] = store_nmf_info(V, Wcon, Hcon, [], options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('MU-Conv (%s): Epoch = 0000, cost = %.16e, optgap = %.4e\n', sub_mode, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        % update H sequentially
        for i = 1 : n

            num = zeros(rank, 1);
            denom = num;
            if i <= n-t+1
                for n_prime = i : i+t-1
                    num = num + W(:, :, n_prime-i+1)' * ((V(:, n_prime) + eps) .* (V_hat(:, n_prime) + eps) .^ (options.d_beta - 2));
                    denom = denom + W(:, :, n_prime-i+1)'*(V_hat(:, n_prime)+eps).^(options.d_beta-1);
                end
            else
                for n_prime = i : n
                    num = num + W(:, :, n_prime-i+1)' * ((V(:, n_prime) + eps) .* (V_hat(:, n_prime) + eps) .^ (options.d_beta - 2));
                    denom = denom + W(:, :, n_prime-i+1)'*(V_hat(:, n_prime) + eps) .^ (options.d_beta-1);
                end
            end

            if strcmp(options.alg, 'type1')
                h_n_old = H(:, i);
                H(:, i) = H(:, i) .* (num./denom) .^ gamma_beta(options.d_beta);

                if i <= n-t+1
                    for n_prime = i:i+t-1
                        V_hat(:, n_prime) = max(V_hat(:, n_prime) + W(:, :, n_prime - i + 1) * (H(:, i) - h_n_old), 0);
                        % max(.,0) ensures the nonnegativity
                    end
                else
                    for n_prime = i:n
                        V_hat(:, n_prime) = max(V_hat(:, n_prime) + W(:, :, n_prime - i + 1) * (H(:, i) - h_n_old), 0);
                    end
                end
            else
                H(:, i) = H(:, i) .* (num./denom) .^ gamma_beta(options.d_beta);
            end

        end

        if strcmp(options.alg, 'type2')

            V_hat = zeros(m, n);
            for i = 0 : t-1
                V_hat = V_hat + W(:, :, i+1) * shift_t(H, i);
            end
        end

        % update W
        for i = 0 : t - 1
            W_t_old = W(:, :, i+1);
            H_shift_t = shift_t(H, i);
            W(:, :, i+1) = W(:, :, i+1) .* (((((V + eps) .* (V_hat + eps) .^ (options.d_beta - 2)) * H_shift_t') ...
                ./ ((V_hat + eps).^(options.d_beta - 1) * H_shift_t'))) .^ gamma_beta(options.d_beta);
            V_hat = max(V_hat + (W(:, :, i+1) - W_t_old) * H_shift_t, 0);
            % max(.,0) ensures nonnegativity
        end

        [W, H] = renormalize_convNMF(W,H);

        if strcmp(options.alg, 'type1')
            % recalate the V_hat
            X_hat = zeros(size(V));
            for i=0:t-1
                tW = W(:, :, i+1);
                tH = shift_t(H, i);
                X_hat = X_hat + tW * tH;
            end
            V_hat = max(X_hat,0);
        end

        % measure gradient calc count
        grad_calc_count = grad_calc_count + m*n;

        % measure elapsed time
        elapsed_time = toc(start_time);

        % update epoch
        epoch = epoch + 1;

        % store info
        [Wcon, Hcon] = reconstruct_wh(W, H, t);
        infos = store_nmf_info(V, Wcon, Hcon, [], options, infos, epoch, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, epoch, infos, options);

    end

    x.W = W;
    x.H = H;

end

function [W_concat, H_concat] = reconstruct_wh(W, H, t)

    W_concat = [];
    H_concat = [];
    for j = 1 : t
        W_concat = [W_concat W(:, :,j)];
        H_concat = [H_concat; shift_t(H, j-1)];
    end
end


%% FUNCTION Logistic_Trace
%   Trace-Norm Regularized Learning with Logistic Loss.
%
%% OBJECTIVE
%   argmin_{W,C} { sum_i^t (- sum(log (1./ (1+ exp(-X{i}*W(:, i) - Y{i} .* C(i)))))/length(Y{i}))
%            + rho1 * \|W\|_*}
%  where \|W\|_* = sum(svd(W, 0)) is the trace norm
%
%% INPUT
%   X: {n * d} * t - input matrix
%   Y: {n * 1} * t - output matrix
%   rho1: trace norm regularization parameter
%
%% OUTPUT
%   W: model: d * t
%   C: model: 1 * t
%   funcVal: function value vector.
%
%% LICENSE
%   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 3 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, see <http://www.gnu.org/licenses/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%
%% RELATED PAPERS
%
%   [1] Ji, S. and Ye, J. An Accelerated Gradient Method for Trace Norm Minimization, ICML 2009
%
%% RELATED FUNCTIONS
%  Least_Trace, init_opts

%% Code starts here
function [W, C, funcVal] = Logistic_Trace(X, Y, rho1, opts)

if nargin <3
    error('\n Inputs: X, Y, and rho1 should be specified!\n');
end
X = multi_transpose(X);

if nargin <4
    opts = [];
end

% initialize options.
opts=init_opts(opts);

if isfield(opts, 'rho_L2')
    rho_L2 = opts.rho_L2;
else
    rho_L2 = 0;
end

task_num  = length (X);
dimension = size(X{1}, 1);
funcVal = [];

%initialize a starting point
C0_prep = zeros(1, task_num);
for t_idx = 1: task_num
    m1 = nnz(Y{t_idx} == 1);
    m2 = nnz(Y{t_idx} == -1);
    if ( m1==0 || m2==0 )
        C0_prep(t_idx) = 0;
    else
        C0_prep(t_idx) = log(m1/m2);
    end
end

if opts.init==2
    W0 = zeros(dimension, task_num);
    C0 = zeros(1, task_num);
elseif opts.init== 0
    W0 = randn(dimension, task_num);
    C0 = C0_prep;
else
    if isfield(opts,'W0')
        W0=opts.W0;
        if (nnz(size(W0)-[dimension, task_num]))
            error('\n Check the input .W0');
        end
    else
        W0 = zeros(dimension, task_num);
    end
    if isfield(opts,'C0')
        C0=opts.C0;
    else
        C0=C0_prep;
    end
end

bFlag=0; % this flag tests whether the gradient step only changes a little

Wz= W0;
Cz= C0;
Wz_old = W0;
Cz_old = C0;

t = 1;
t_old = 0;
iter = 0;
gamma = 1;
gamma_inc = 2;

while iter < opts.maxIter
    alpha = (t_old - 1) /t;

    Ws = (1 + alpha) * Wz - alpha * Wz_old;
    Cs = (1 + alpha) * Cz - alpha * Cz_old;

    % compute function value and gradients of the search point
    [gWs, gCs, Fs ]  = gradVal_eval(Ws, Cs);

    % the Armijo Goldstein line search scheme
    while true
        [Wzp Wzp_tn] = trace_projection(Ws - gWs/gamma, 2 * rho1 / gamma);
        Czp = Cs - gCs/gamma;
        Fzp = funVal_eval  (Wzp, Czp);

        delta_Wzp = Wzp - Ws;
        delta_Czp = Czp - Cs;
        nrm_delta_Wzp = norm(delta_Wzp, 'fro')^2;
        nrm_delta_Czp = norm(delta_Czp, 'fro')^2;
        r_sum = (nrm_delta_Wzp+nrm_delta_Czp)/2;

        %         Fzp_gamma = Fs + trace(delta_Wzp' * gWs)...
        %             + trace(delta_Czp' * gCs)...
        %             + gamma/2 * nrm_delta_Wzp ...
        %             + gamma/2 * nrm_delta_Czp;
        Fzp_gamma = Fs + sum(sum(delta_Wzp .* gWs))...
            + sum(sum(delta_Czp .* gCs))...
            + gamma/2 * nrm_delta_Wzp ...
            + gamma/2 * nrm_delta_Czp;

        if (r_sum <=1e-20)
            bFlag=1; % this shows that, the gradient step makes little improvement
            break;
        end

        if (Fzp <= Fzp_gamma)
            break;
        else
            gamma = gamma * gamma_inc;
        end
    end

    Wz_old = Wz;
    Cz_old = Cz;
    Wz = Wzp;
    Cz = Czp;

    %funcVal = cat(1, funcVal, Fzp + rho1 * sum( svd(Wzp, 0) ));
    funcVal = cat(1, funcVal, Fzp + rho1 * Wzp_tn);

    if (bFlag)
        % fprintf('\n The program terminates as the gradient step changes the solution very small.');
        break;
    end

    % test stop condition.
    switch(opts.tFlag)
        case 0
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <= opts.tol)
                    break;
                end
            end
        case 1
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <=...
                        opts.tol* funcVal(end-1))
                    break;
                end
            end
        case 2
            if ( funcVal(end)<= opts.tol)
                break;
            end
        case 3
            if iter>=opts.maxIter
                break;
            end
    end

    iter = iter + 1;
    t_old = t;
    t = 0.5 * (1 + (1+ 4 * t^2)^0.5);

end

W = Wzp;
C = Czp;

% private functions
    function [grad_W, grad_C, funcVal] = gradVal_eval(W, C)
        grad_W = zeros(dimension, task_num);
        grad_C = zeros(1, task_num);
        lossValVect = zeros (1 , task_num);
        if opts.pFlag
            parfor i = 1:task_num
                [ grad_W(:, i), grad_C(:, i), lossValVect(:, i)] = unit_grad_eval( W(:, i), C(i), X{i}, Y{i});
            end
        else
            for i = 1:task_num
                [ grad_W(:, i), grad_C(:, i), lossValVect(:, i)] = unit_grad_eval( W(:, i), C(i), X{i}, Y{i});
            end
        end
        grad_W = grad_W+ rho_L2 * 2 * W;
        % here when computing function value we do not include
        % l1 norm.
        funcVal = sum(lossValVect) + rho_L2 * norm(W, 'fro')^2;
    end

    function [funcVal] = funVal_eval (W, C )
        funcVal = 0;
        if opts.pFlag
            parfor i = 1: task_num
                funcVal = funcVal + unit_funcVal_eval( W(:, i), C(i), X{i}, Y{i});
            end
        else
            for i = 1: task_num
                funcVal = funcVal + unit_funcVal_eval( W(:, i), C(i), X{i}, Y{i});
            end
        end
        % here when computing function value we do not include
        % l1 norm.
        funcVal = funcVal + rho_L2 * norm(W, 'fro')^2;
    end

end

function [ grad_w, grad_c, funcVal ] = unit_grad_eval( w, c, x, y)
%gradient and logistic evaluation for each task
m = length(y);
weight = ones(m, 1)/m;
weighty = weight.* y;
aa = -y.*(x'*w + c);
bb = max( aa, 0);
funcVal = weight'* ( log( exp(-bb) +  exp(aa-bb) ) + bb );
pp = 1./ (1+exp(aa));
b = -weighty.*(1-pp);
grad_c = sum(b);
grad_w = x * b;
end

function [ funcVal ] = unit_funcVal_eval( w, c, x, y)
%function value evaluation for each task
m = length(y);
weight = ones(m, 1)/m;
aa = -y.*(x'*w + c);
bb = max( aa, 0);
funcVal = weight'* ( log( exp(-bb) +  exp(aa-bb) ) + bb );
end


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

    q = double(q);
    s = q(1);
    x = q(2);
    y = q(3);
    z = q(4);

    r = [   1-2*(y^2+z^2)   2*(x*y-s*z) 2*(x*z+s*y)
        2*(x*y+s*z) 1-2*(x^2+z^2)   2*(y*z-s*x)
        2*(x*z-s*y) 2*(y*z+s*x) 1-2*(x^2+y^2)   ];
    t = eye(4,4);
    t(1:3,1:3) = r;
    t(4,4) = 1;
endfunction


function failed_old_specest_vs_oldimplementation

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY

% read in data from /home/common/testdata (I just my own testdata)
% data = .....

%% COHERENCE specest_mtmfft
cfg = [];
cfg.keeptrials = 'yes';
cfg.foilim     = [2 100];
cfg.output     = 'powandcsd';
cfg.taper      = 'dpss';
cfg.tapsmofrq  = 4;
cfg.pad        = 'maxperlen';
cfg.calcdof    = 'yes';
cfg.keeptrials = 'no';
% old style
cfg.method     = 'mtmfft_old';
tic; freqold = ft_freqanalysis(cfg,data); toc
% new style
cfg.method     = 'mtmfft';
tic; freqnew = ft_freqanalysis(cfg,data); toc

% calculate coherence
cfg = [];
cfg.method = 'coh';
connold = ft_connectivityanalysis(cfg,freqold);
connnew = ft_connectivityanalysis(cfg,freqnew);

% plot 'a' chancomb
figure
subplot(3,1,1)
plot(connold.freq,squeeze(connold.cohspctrm(10,:)))
axis tight
xlabel('frequency (Hz)')
ylabel('coh')
title('old implementation')
subplot(3,1,2)
plot(connnew.freq,squeeze(connnew.cohspctrm(10,:)))
axis tight
xlabel('frequency (Hz)')
ylabel('coh')
title('new implementation')
subplot(3,1,3)
hold on
plot(connold.freq,squeeze(connold.cohspctrm(10,:)))
plot(connnew.freq,squeeze(connnew.cohspctrm(10,:)),'r')
legend
axis tight
xlabel('frequency (Hz)')
ylabel('coh')
title('both overlayed, using freq axis of new implementation')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% POWER specest_mtmfft
cfg = [];
cfg.trials     = 1:3;
cfg.keeptrials = 'yes';
cfg.foilim     = [0 256/2];
cfg.output     = 'pow';
cfg.taper      = 'dpss';
cfg.tapsmofrq  = 4;
cfg.pad        = 'maxperlen';
cfg.calcdof    = 'yes';
% old style
cfg.method     = 'mtmfft_old';
tic; freqold = ft_freqanalysis(cfg,data); toc
% new style
cfg.method     = 'mtmfft';
tic; freqnew = ft_freqanalysis(cfg,data); toc

% plot first trial first channel, single timepoint
figure
subplot(3,1,1)
plot(freqold.freq,squeeze(freqold.powspctrm(1,1,:)))
axis tight
xlabel('frequency (Hz)')
ylabel('power')
title('old implementation')
subplot(3,1,2)
plot(freqnew.freq,squeeze(freqnew.powspctrm(1,1,:)))
axis tight
xlabel('frequency (Hz)')
ylabel('power')
title('new implementation')
subplot(3,1,3)
hold on
plot(freqnew.freq,squeeze(freqold.powspctrm(1,1,:)))
plot(freqnew.freq,squeeze(freqnew.powspctrm(1,1,:)),'r')
legend
axis tight
xlabel('frequency (Hz)')
ylabel('power')
title('both overlayed, using freq axis of new implementation')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% COHERENCE specest_mtmconvol
cfg = [];
cfg.trials     = 1:2;
cfg.keeptrials = 'no';
cfg.keeptapers = 'no';
cfg.channel    = 'all';
cfg.channelcmb = {'all' 'all'};
%cfg.pad        = 4.24;
cfg.foi        = 2:2:20;
cfg.toi        = data.time{1}(1):0.050:data.time{1}(end);
cfg.output     = 'powandcsd';
cfg.taper      = 'hanning';
cfg.t_ftimwin  = 4 ./ cfg.foi; %ones(length(cfg.foi),1) .* 1.5; %
cfg.tapsmofrq  = ones(length(cfg.foi),1) * 8;
cfg.calcdof    = 'yes';
cfg.correctt_ftimwin = 'no';
% old style
cfg.method     = 'mtmconvol_old';
tic; freqold = ft_freqanalysis(cfg,data); toc
% new style
cfg.method     = 'mtmconvol';
tic; freqnew = ft_freqanalysis(cfg,data); toc

% calculate coherence
cfg = [];
cfg.method = 'coh';
connold = ft_connectivityanalysis(cfg,freqold);
connnew = ft_connectivityanalysis(cfg,freqnew);

% plot 'a' chancomb at a single timepoint
timepoint = 20;
figure
subplot(3,1,1)
plot(connold.freq,squeeze(connold.cohspctrm(10,:,timepoint)))
axis tight
xlabel('frequency (Hz)')
ylabel('coh')
title('old implementation')
subplot(3,1,2)
plot(connnew.freq,squeeze(connnew.cohspctrm(10,:,timepoint)))
axis tight
xlabel('frequency (Hz)')
ylabel('coh')
title('new implementation')
subplot(3,1,3)
hold on
plot(connold.freq,squeeze(connold.cohspctrm(10,:,timepoint)))
plot(connnew.freq,squeeze(connnew.cohspctrm(10,:,timepoint)),'r')
legend
axis tight
xlabel('frequency (Hz)')
ylabel('coh')
title('both overlayed, using freq axis of new implementation')

% plot 'a' chancomb at a all timepoints
figure
subplot(3,1,1)
imagesc(connold.time,connold.freq,squeeze(connold.cohspctrm(10,:,:)))
axis tight
ylabel('frequency (Hz)')
xlabel('time (s)')
title('old implementation')
colorbar; axis xy; caxis([0 1])
subplot(3,1,2)
imagesc(connnew.time,connnew.freq,squeeze(connnew.cohspctrm(10,:,:)))
axis tight
ylabel('frequency (Hz)')
xlabel('time (s)')
title('new implementation')
colorbar; axis xy; caxis([0 1]);
subplot(3,1,3)
hold on
imagesc(connold.time,connold.freq,squeeze(connold.cohspctrm(10,:,:))-squeeze(connnew.cohspctrm(10,:,:)))
legend
axis tight
ylabel('frequency (Hz)')
xlabel('time (s)')
title('old minus new')
colorbar; caxis([0 1]); axis xy
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% POWER specest_mtmconvol
cfg = [];
cfg.implementation = 'old';
cfg.trials     = 1:3;
cfg.keeptrials = 'yes';
cfg.keeptapers = 'no';
%cfg.pad        = 4.24;
cfg.foi        = 0:2:128;
cfg.toi        = data.time{1}(1):0.0050:data.time{1}(end);
cfg.output     = 'pow';
cfg.taper      = 'hanning';
cfg.t_ftimwin  = 4 ./ cfg.foi; %ones(length(cfg.foi),1) .* 1.5; %
cfg.tapsmofrq  = ones(length(cfg.foi),1) * 8;
cfg.calcdof    = 'yes';
cfg.correctt_ftimwin = 'no';
% new style
cfg.method     = 'mtmconvol';
tic; freqnew = ft_freqanalysis(cfg,data); toc
% old style
cfg.method     = 'mtmconvol_old';
cfg.foi = freqnew.freq;
cfg.t_ftimwin  = 4 ./ cfg.foi; %ones(length(cfg.foi),1) .* 1.5; %
tic; freqold = ft_freqanalysis(cfg,data); toc

% plot first trial first channel, single timepoint
timepoint = 20;
figure
subplot(3,1,1)
plot(freqold.freq,squeeze(freqold.powspctrm(1,1,:,timepoint)))
axis tight
xlabel('frequency (Hz)')
ylabel('power')
title('old implementation')
subplot(3,1,2)
plot(freqnew.freq,squeeze(freqnew.powspctrm(1,1,:,timepoint)))
axis tight
xlabel('frequency (Hz)')
ylabel('power')
title('new implementation')
subplot(3,1,3)
hold on
plot(freqnew.freq,squeeze(freqold.powspctrm(1,1,:,timepoint)))
plot(freqnew.freq,squeeze(freqnew.powspctrm(1,1,:,timepoint)),'r')
legend
axis tight
xlabel('frequency (Hz)')
ylabel('power')
title('both overlayed, using freq axis of new implementation')

% plot first trial first channel, all timepoints
figure
subplot(3,1,1)
imagesc(freqold.time,freqold.freq,squeeze(freqold.powspctrm(1,1,:,:)))
axis tight; axis xy; colorbar
ylabel('frequency (Hz)')
xlabel('time (s)')
title('old implementation')
subplot(3,1,2)
imagesc(freqold.time,freqnew.freq,squeeze(freqnew.powspctrm(1,1,:,:)))
axis tight; axis xy; colorbar
ylabel('frequency (Hz)')
xlabel('time (s)')
title('new implementation')
subplot(3,1,3)
hold on
imagesc(freqold.time,freqold.freq,squeeze(freqold.powspctrm(1,1,:,:)) - squeeze(freqnew.powspctrm(1,1,:,:)))
legend
axis tight; axis xy; colorbar
ylabel('frequency (Hz)')
xlabel('time (s)')
title('old minus new')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% POWER specest_wavelet
cfg = [];
cfg.trials     = 1;
cfg.keeptrials = 'yes';
cfg.keeptapers = 'no';
%cfg.pad        = 4.24;
cfg.foi        = 2:1:40;
cfg.toi        = 0:0.05:4; %data.time{1}(1):(1/data.fsample):data.time{1}(end);
cfg.output     = 'pow';
cfg.calcdof    = 'yes';
cfg.correctt_ftimwin = 'no';
% new style
cfg.method     = 'wavelet';
tic; freqnew = ft_freqanalysis(cfg,data); toc
% old style
cfg.foi = freqnew.freq;
cfg.method     = 'wltconvol_old';
tic; freqold = ft_freqanalysis(cfg,data); toc

% plot first trial first channel, single timepoint
timepoint = 40;
figure
subplot(3,1,1)
plot(freqold.freq,squeeze(freqold.powspctrm(1,1,:,timepoint)))
axis tight
xlabel('frequency (Hz)')
ylabel('power')
title('old implementation')
subplot(3,1,2)
plot(freqnew.freq,squeeze(freqnew.powspctrm(1,1,:,timepoint)))
axis tight
xlabel('frequency (Hz)')
ylabel('power')
title('new implementation')
subplot(3,1,3)
hold on
plot(freqnew.freq,squeeze(freqold.powspctrm(1,1,:,timepoint)))
plot(freqnew.freq,squeeze(freqnew.powspctrm(1,1,:,timepoint)),'r')
legend
axis tight
xlabel('frequency (Hz)')
ylabel('power')
title('both overlayed, using freq axis of new implementation')

% plot first trial first channel, all timepoints
figure
subplot(3,1,1)
imagesc(freqold.time,freqold.freq,squeeze(freqold.powspctrm(1,1,:,:)))
axis tight; axis xy; colorbar
ylabel('frequency (Hz)')
xlabel('time (s)')
title('old implementation')
subplot(3,1,2)
imagesc(freqold.time,freqnew.freq,squeeze(freqnew.powspctrm(1,1,:,:)))
axis tight; axis xy; colorbar
ylabel('frequency (Hz)')
xlabel('time (s)')
title('new implementation')
subplot(3,1,3)
hold on
imagesc(freqold.time,freqold.freq,squeeze(freqold.powspctrm(1,1,:,:)) - squeeze(freqnew.powspctrm(1,1,:,:)))
legend
axis tight; axis xy; colorbar
ylabel('frequency (Hz)')
xlabel('time (s)')
title('old minus new')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% CSD specest_wavelet
cfg = [];
cfg.trials     = 'all';
cfg.keeptrials = 'yes';
cfg.keeptapers = 'no';
%cfg.pad        = 4.24;
cfg.foi        = 2:2:40;
cfg.toi        = data.time{1}(1):(1/data.fsample):data.time{1}(end);
cfg.output     = 'powandcsd';
cfg.calcdof    = 'yes';
cfg.correctt_ftimwin = 'no';
% new style
cfg.method     = 'wavelet';
tic; freqnew = ft_freqanalysis(cfg,data); toc
% old style
cfg.foi = freqnew.freq;
cfg.method     = 'wltconvol_old';
tic; freqold = ft_freqanalysis(cfg,data); toc

% plot first trial first channel, single timepoint
timepoint = 200;
figure
subplot(3,1,1)
plot(freqold.freq,abs(squeeze(freqold.crsspctrm(1,1,:,timepoint))))
axis tight
xlabel('frequency (Hz)')
ylabel('')
title('old implementation')
subplot(3,1,2)
plot(freqnew.freq,abs(squeeze(freqnew.crsspctrm(1,1,:,timepoint))))
axis tight
xlabel('frequency (Hz)')
ylabel('')
title('new implementation')
subplot(3,1,3)
hold on
plot(freqnew.freq,abs(squeeze(freqold.crsspctrm(1,1,:,timepoint))))
plot(freqnew.freq,abs(squeeze(freqnew.crsspctrm(1,1,:,timepoint))),'r')
legend
axis tight
xlabel('frequency (Hz)')
ylabel('')
title('both overlayed, using freq axis of new implementation')

% plot first trial first channel, all timepoints
figure
subplot(3,1,1)
imagesc(freqold.time,freqold.freq,abs(squeeze(freqold.crsspctrm(1,1,:,:))))
axis tight; axis xy; colorbar
ylabel('frequency (Hz)')
xlabel('time (s)')
title('old implementation')
subplot(3,1,2)
imagesc(freqold.time,freqnew.freq,abs(squeeze(freqnew.crsspctrm(1,1,:,:))))
axis tight; axis xy; colorbar
ylabel('frequency (Hz)')
xlabel('time (s)')
title('new implementation')
subplot(3,1,3)
hold on
imagesc(freqold.time,freqold.freq,abs(squeeze(freqold.crsspctrm(1,1,:,:)) - squeeze(freqnew.crsspctrm(1,1,:,:))))
legend
axis tight; axis xy; colorbar
ylabel('frequency (Hz)')
xlabel('time (s)')
title('old minus new')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% COHERENCE specest_wavelet
cfg = [];
cfg.trials     = 'all';
cfg.keeptrials = 'yes';
cfg.keeptapers = 'no';
cfg.channel    = 'all';
cfg.channelcmb = {'all' 'all'};
%cfg.pad        = 4.24;
cfg.foi        = 2:2:20;
cfg.toi        = data.time{1}(1):0.050:data.time{1}(end);
cfg.output     = 'powandcsd';
cfg.calcdof    = 'yes';
% new style
cfg.method     = 'wavelet';
tic; freqnew = ft_freqanalysis(cfg,data); toc
% old style
cfg.foi = freqnew.freq;
cfg.method     = 'wltconvol_old';
tic; freqold = ft_freqanalysis(cfg,data); toc

% calculate coherence
cfg = [];
cfg.method = 'coh';
connold = ft_connectivityanalysis(cfg,freqold);
connnew = ft_connectivityanalysis(cfg,freqnew);

% plot 'a' chancomb at a single timepoint
timepoint = 20;
figure
subplot(3,1,1)
plot(connold.freq,squeeze(connold.cohspctrm(10,:,timepoint)))
axis tight
xlabel('frequency (Hz)')
ylabel('coh')
title('old implementation')
subplot(3,1,2)
plot(connnew.freq,squeeze(connnew.cohspctrm(10,:,timepoint)))
axis tight
xlabel('frequency (Hz)')
ylabel('coh')
title('new implementation')
subplot(3,1,3)
hold on
plot(connold.freq,squeeze(connold.cohspctrm(10,:,timepoint)))
plot(connnew.freq,squeeze(connnew.cohspctrm(10,:,timepoint)),'r')
legend
axis tight
xlabel('frequency (Hz)')
ylabel('coh')
title('both overlayed, using freq axis of new implementation')

% plot 'a' chancomb at a all timepoints
figure
subplot(3,1,1)
imagesc(connold.time,connold.freq,squeeze(connold.cohspctrm(10,:,:)))
axis tight
ylabel('frequency (Hz)')
xlabel('time (s)')
title('old implementation')
colorbar; axis xy; caxis([0 1])
subplot(3,1,2)
imagesc(connnew.time,connnew.freq,squeeze(connnew.cohspctrm(10,:,:)))
axis tight
ylabel('frequency (Hz)')
xlabel('time (s)')
title('new implementation')
colorbar; axis xy; caxis([0 1]);
subplot(3,1,3)
hold on
imagesc(connold.time,connold.freq,squeeze(connold.cohspctrm(10,:,:))-squeeze(connnew.cohspctrm(10,:,:)))
legend
axis tight
ylabel('frequency (Hz)')
xlabel('time (s)')
title('old minus new')
colorbar; caxis([0 1]); axis xy
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% POWER specest_hilbert
cfg = [];
cfg.trials     = 1:3;
cfg.keeptrials = 'yes';
cfg.keeptapers = 'no';
cfg.width      = 1;
cfg.pad        = 'maxperlen';
cfg.foi        = 10:2:40;
cfg.toi        = 0:0.05:4; %data.time{1}(1):(1/data.fsample):data.time{1}(end);
cfg.output     = 'pow';
cfg.calcdof    = 'yes';
cfg.correctt_ftimwin = 'no';
% new style
cfg.method     = 'hilbert';
cfg.filtorder  = round((3 ./ cfg.foi) .* data.fsample);
cfg.filttype   = 'fir';
cfg.width      = 5;

tic; freqnew = ft_freqanalysis(cfg,data); toc
% old style
cfg.foi = freqnew.freq;
cfg.method     = 'mtmconvol';
cfg.taper      = 'hanning';
cfg.t_ftimwin  = 3 ./ cfg.foi; %ones(length(cfg.foi),1) .* 1.5; %
tic; freqold = ft_freqanalysis(cfg,data); toc

% plot first trial first channel, single timepoint
timepoint = 40;
figure
subplot(3,1,1)
plot(freqold.freq,squeeze(freqold.powspctrm(1,1,:,timepoint)))
axis tight
xlabel('frequency (Hz)')
ylabel('power')
title('mtmconvol')
subplot(3,1,2)
plot(freqnew.freq,squeeze(freqnew.powspctrm(1,1,:,timepoint)))
axis tight
xlabel('frequency (Hz)')
ylabel('power')
title('hilbert')
subplot(3,1,3)
hold on
plot(freqnew.freq,squeeze(freqold.powspctrm(1,1,:,timepoint)))
plot(freqnew.freq,squeeze(freqnew.powspctrm(1,1,:,timepoint)),'r')
legend
axis tight
xlabel('frequency (Hz)')
ylabel('power')
title('both overlayed, using freq axis of hilbert')

% plot first trial first channel, all timepoints
figure
subplot(3,1,1)
imagesc(freqold.time,freqold.freq,squeeze(freqold.powspctrm(1,1,:,:)))
axis tight; axis xy; colorbar
ylabel('frequency (Hz)')
xlabel('time (s)')
title('mtmconvol')
subplot(3,1,2)
imagesc(freqold.time,freqnew.freq,squeeze(freqnew.powspctrm(1,1,:,:)))
axis tight; axis xy; colorbar
ylabel('frequency (Hz)')
xlabel('time (s)')
title('hilbert')
subplot(3,1,3)
hold on
imagesc(freqold.time,freqold.freq,squeeze(freqold.powspctrm(1,1,:,:)) - squeeze(freqnew.powspctrm(1,1,:,:)))
legend
axis tight; axis xy; colorbar
ylabel('frequency (Hz)')
xlabel('time (s)')
title('old minus new')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% FOURIER specest_hilbert
cfg = [];
cfg.trials     = 1:3;
cfg.keeptrials = 'yes';
cfg.keeptapers = 'yes';
cfg.width      = 1;
cfg.pad        ='maxperlen';
cfg.foi        = 10:2:40;
cfg.toi        = 0:0.05:4; %data.time{1}(1):(1/data.fsample):data.time{1}(end);
cfg.output     = 'fourier';
cfg.calcdof    = 'yes';
cfg.correctt_ftimwin = 'no';
% new style
cfg.method     = 'hilbert';
cfg.filtorder  = round((3 ./ cfg.foi) .* data.fsample);
cfg.filttype   = 'fir';
cfg.width      = 1;
tic; freqnew = ft_freqanalysis(cfg,data); toc
% old style
cfg.foi = freqnew.freq;
cfg.method     = 'mtmconvol';
cfg.taper      = 'hanning';
cfg.t_ftimwin  = 3 ./ cfg.foi; %ones(length(cfg.foi),1) .* 1.5; %
tic; freqold = ft_freqanalysis(cfg,data); toc

% plot first trial first channel, real part
freq = 3;
figure
subplot(3,1,1)
plot(freqold.time,real(squeeze(freqold.fourierspctrm(1,1,freq,:))))
axis tight
xlabel('frequency (Hz)')
ylabel('amplitude')
title('mtmconvol')
subplot(3,1,2)
plot(freqnew.time,real(squeeze(freqnew.fourierspctrm(1,1,freq,:))))
axis tight
xlabel('frequency (Hz)')
ylabel('amplitude')
title('hilbert')
subplot(3,1,3)
hold on
plot(freqnew.time,real(squeeze(freqold.fourierspctrm(1,1,freq,:))))
plot(freqnew.time,real(squeeze(freqnew.fourierspctrm(1,1,freq,:))),'r')
legend
axis tight
xlabel('frequency (Hz)')
ylabel('amplitude')
title('both overlayed, using freq axis of hilbert')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% FOURIER specest_wavelet VS specest_tfr
cfg = [];
cfg.trials     = 1;
cfg.keeptrials = 'yes';
cfg.keeptapers = 'yes';
cfg.pad        = 6;
cfg.foi        = 2:5:20;
cfg.toi        = 0:0.05:4; %data.time{1}(1):(1/data.fsample):data.time{1}(end);
cfg.output     = 'fourier';
cfg.calcdof    = 'yes';
cfg.correctt_ftimwin = 'no';
% new style
cfg.method     = 'wavelet';
tic; freqwav = ft_freqanalysis(cfg,data); toc
% old style
cfg.foi = freqwav.freq;
cfg.method     = 'tfr';
tic; freqtfr = ft_freqanalysis(cfg,data); toc

% plot first trial first channel, single timepoint
timepoint = 50;
figure
subplot(4,1,1)
plot(freqtfr.freq,real(squeeze(freqtfr.fourierspctrm(1,1,:,timepoint))))
axis tight
xlabel('frequency (Hz)')
ylabel('amplitude')
title('tfr implementation')
subplot(4,1,2)
plot(freqwav.freq,real(squeeze(freqwav.fourierspctrm(1,1,:,timepoint))))
axis tight
xlabel('frequency (Hz)')
ylabel('amplitude')
title('wavelet implementation')
subplot(4,1,3)
plot(freqtfr.freq,angle(squeeze(freqtfr.fourierspctrm(1,1,:,timepoint))))
axis tight
xlabel('frequency (Hz)')
ylabel('phase')
title('tfr implementation')
subplot(4,1,4)
plot(freqwav.freq,angle(squeeze(freqwav.fourierspctrm(1,1,:,timepoint))))
axis tight
xlabel('frequency (Hz)')
ylabel('phase')
title('wavelet implementation')


addpath('/home/liisa/080812/');
addpath('/home/rheintz/matlab/');

global para;

s=19;

% beadsAt=[180 504; 207 489; 253 236; 399 276; 262 54; 364 49; 393 168; 410 156; 300 139; 190 138; 376 282; 182 459; 333 449; 367 467; 415 453; 204 550; 175 579];

myres=para.res_resIm
fprintf('Please select bead coordinates by left clicking. Finish with right click\n');
beadsAt=dipgetcoords(100);
fprintf('%d beads selected\n',size(beadsAt,1));
beadsAt(end,:)=[];

[sumBdsHighRes,mybdsHRes,sumBdsSum,mybdsSum]=MeanFromCoord(myres,beadsAt,s,para.res_sumIm)

%myresSum=para.res_sumIm;
%[sumBdsSum,mybdsSum]=MeanFromCoord(myresSum,beadsAt,s)

%im = readtimeseries('beads1_000');
%myres=squeeze(sum(im(:,:,3:5)));
%[sumBdsRaw,mybdsRaw]=MeanFromCoord(myres,beadsAt,s)

% [params,res,fitted]=FitDataNDFast([10 10 0; 1000 s/2 s/2],sumBdsSum,300,'idiv')

[fittedSum,paramsSum,idiv,myfunct] = FitDataND('c(1)*exp(-((x{1}-c(2)).^2+(x{2}-c(3)).^2)/c(4))+c(5)',[max(sumBdsSum) 0 0 20 min(sumBdsSum)],sumBdsSum,1000);
sqrt(paramsSum(4))*para.res_resampledPixelsize(1) * 2*sqrt(log(2))  % to get FWHM
sumBdsSum-fittedSum

[fittedHRes,paramsHRes,idiv,myfunct] = FitDataND('c(1)*exp(-((x{1}-c(2)).^2+(x{2}-c(3)).^2)/c(4))+c(5)',[max(sumBdsHighRes) 0 0 20 min(sumBdsHighRes)],sumBdsHighRes,1000);
sqrt(paramsHRes(4))*para.res_resampledPixelsize(1)* 2*sqrt(log(2))  % to get FWHM
sumBdsHighRes-fittedHRes


% SP_BSPLINE_FLUID: Construct different pair of B-Splines spaces on the physical domain for fluid problems.
%
%   [spv, spp] = sp_bspline_fluid (elem_name, knots, nsub, ...
%                                               degreep, regularity, msh)
%
% INPUTS:
%
%   elem_name:  the name of the element. Right now 'TH' (Taylor-Hood),
%                'NDL' (Nedelec, 2nd family), 'RT' (Raviart-Thomas) and
%                'SG' (SubGrid) are supported.
%   knots:      knot vector of the coarse geometry.
%   nsub:       number of subdivisions of each interval.
%   degreep:    degree of the pressure space along each parametric direction
%   regularity: continuity of the pressure space along each parametric direction
%   msh:        msh object containing (in the field msh.qn) the points
%                along each parametric direction in the parametric
%                domain at which to evaluate, i.e. quadrature points
%                or points for visualization (see msh_cartesian).
%
% OUTPUT:
%
%   spv: object representing the discrete velocity function space (see sp_vector)
%   spp: object representing the discrete pressure function space (see sp_scalar)
%
%   For more details, see:
%      A. Buffa, C. de Falco, G. Sangalli,
%      IsoGeometric Analysis: Stable elements for the 2D Stokes equation
%      Internat. J. Numer. Methods Fluids, 2011
%
%      A. Bressan, G. Sangalli,
%      Isogeometric discretizations of the Stokes problem: stability
%       analysis by the macroelement technique
%      IMA J. Numer. Anal., 2013.
%
% Copyright (C) 2009, 2010, 2011 Carlo de Falco
% Copyright (C) 2011, 2015 Rafael Vazquez
%
%    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 3 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, see <http://www.gnu.org/licenses/>.

function [spv, spp] = sp_bspline_fluid (element_name, ...
                   knots, nsub_p, degree_p, regularity_p, msh)

% Construction of the knot vectors and the discrete space for the velocity
switch (lower (element_name))
  case {'th'}
    knotsp = kntrefine (knots, nsub_p-1, degree_p, regularity_p);
    spp = sp_bspline (knotsp, degree_p, msh);

    degree_v = degree_p + 1;
    regularity_v = regularity_p;
    nsub_v = nsub_p;
    knots_v = kntrefine (knots, nsub_v-1, degree_v, regularity_v);
    scalar_space = sp_bspline (knots_v, degree_v, msh);
    for idim = 1:msh.ndim
      scalar_spaces{idim} = scalar_space;
    end
    spv = sp_vector (scalar_spaces, msh);
    clear scalar_spaces scalar_space

  case {'sg'}
    knotsp = kntrefine (knots, nsub_p-1, degree_p, regularity_p);
    spp = sp_bspline (knotsp, degree_p, msh);

    degree_v = degree_p + 1;
    regularity_v = regularity_p+1;
    nsub_v = 2*nsub_p;
    knots_v = kntrefine (knots, nsub_v-1, degree_v, regularity_v);
    scalar_space = sp_bspline (knots_v, degree_v, msh);
    for idim = 1:msh.ndim
      scalar_spaces{idim} = scalar_space;
    end
    spv = sp_vector (scalar_spaces, msh);
    clear scalar_spaces scalar_space

  case {'ndl'}
% In this case the regularity is assigned first in the velocity space
    degree_h1 = degree_p + 1;
    regularity_h1 = regularity_p + 1;
    knots_h1 = kntrefine (knots, nsub_p-1, degree_h1, regularity_h1);
    [knots_hdiv, degree_hdiv] = knt_derham (knots_h1, degree_h1, 'Hdiv');

    degree_v = degree_h1;
    for idim = 1:msh.ndim
      knots_v{idim} = knots_hdiv{idim}{idim};
      for jdim = setdiff (1:msh.ndim, idim)
        knots_v{jdim} = sort ([knots_hdiv{idim}{jdim}, unique(knots_hdiv{idim}{jdim})]);
      end
      scalar_spaces{idim} = sp_bspline (knots_v, degree_v, msh);
    end
    spv = sp_vector (scalar_spaces, msh, 'div-preserving');
    clear scalar_spaces

    [knotsp, degp] = knt_derham (knots_h1, degree_h1, 'L2');
    spp = sp_bspline (knotsp, degp, msh, 'integral-preserving');

  case {'rt'}
% In this case the regularity is assigned first in the velocity space
    degree_h1 = degree_p + 1;
    regularity_h1 = regularity_p + 1;
    knots_h1 = kntrefine (knots, nsub_p-1, degree_h1, regularity_h1);

    [knots_v, degree_v] = knt_derham (knots_h1, degree_h1, 'Hdiv');
    for idim = 1:msh.ndim
      scalar_spaces{idim} = sp_bspline (knots_v{idim}, degree_v{idim}, msh);
    end
    spv = sp_vector (scalar_spaces, msh, 'div-preserving');
    clear scalar_spaces

    [knotsp, degp] = knt_derham (knots_h1, degree_h1, 'L2');
    spp = sp_bspline (knotsp, degp, msh, 'integral-preserving');

%     if (nargout == 3)
%       PI = b2nst__ (spp, knotsp, degree_p, msh);
%     end
  otherwise
    error ('sp_bspline_fluid: unknown element type')
end

end


function ct = matRad_calcHU(ct)
% matRad function to calculate Hounsfield units from a dicom ct
% that originally uses intensity values
%
% call
%   ct = matRad_calcHU(ct)
%
% input
%   ct: unprocessed dicom ct data which are stored as intensity values (IV)
%
%                      HU = IV * slope + intercept
%
% output
%   ct:                 ct struct with cube with HU
%
% References
%   -
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2015 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i = 1:ct.numOfCtScen
    ct.cubeHU{i} = double(ct.cubeIV{i}) * double(ct.dicomInfo.RescaleSlope) + double(ct.dicomInfo.RescaleIntercept);
end

ct = rmfield(ct,'cubeIV');

end


% Run experiments for simulated datasets.
% Usage: Q = metoffice_experiment_pca('hadsst2d1', 1, 40, 4)

function [ Q, perfmeas ] = metoffice_experiment_pca(dataset, anomalies, D, remove_val)

%
% Load data
%

if nargin < 4 || isempty(remove_val)
    remove_val = 1;
end
[data,folder,maskfile] = metoffice_getdata(dataset, anomalies, remove_val);

% Form the data matrix
Y = data.data;
[M,N] = size(Y);
Obs = ~isnan(Y);

%
% PCA inference
%

% Number of components
if nargin < 3 || isempty(D)
    D = 120;
end

% Filename for saving the results
%folder = [folder '/pca'];
mkdir(folder);
filename = sprintf('%s/%s/pca_D=%d_anomalies=%d_remval=%02d_%s', ...
                   folder, ...
                   dataset, ...
                   D, ...
                   anomalies, ...
                   remove_val, ...
                   datestr(now,'yyyymmdd'))

% PCA module for X (one constant component for modeling bias)
prior.mu = [1; zeros(D-1,1)];
prior.CovX = diag([1e-6; ones(D-1,1)]);
X_module = factor_module_iid('prior', prior);

% ARD module for W
W_module = factor_module_ard();

% Isotropic noise (precisions weighted proportionally to grid size)
[LON,LAT] = meshgrid(data.lon, data.lat);
weights = cosd(LAT(:));
weights = metoffice_remove_bins(weights,maskfile);
weights = repmat(weights, [1, N]);
noise_module = noise_module_isotropic('init', struct('tau', 10), ...
                                      'weights', weights);

% Run VB PCA
Q = vbfa(D, Y, W_module, X_module, noise_module, ...
         'maxiter', 2, ...
         'rotate', true, ...
         'autosavefile', filename, ...
         'autosave', [1 20:20:2000]);

if nargout == 2
    % Reconstruct
    Yrec = Q.W'*Q.X;

    % Remove the autosave file
    delete( [ filename '.mat' ] )

    addpath /home/alexilin/matlab/metoffice
    Yrec = add_land( Yrec, dataset );
    Yrec = add_climatology( Yrec, dataset, anomalies );
    perfmeas = compute_rmse( Yrec, dataset, remove_val );

else
    % Reconstruct
    Yrec = Q.W'*Q.X;

    % Some performance measures
    fprintf('Weighted training RMSE of the reconstruction: %f\n',  ...
            rmsew(Y(Obs)-Yrec(Obs),weights(Obs)));

end

% Save the results
%save(filename, '-struct', 'Q');


% This file is part of the project NILM-Eval (https://github.com/beckel/nilm-eval).
% Licence: GPL 2.0 (http://www.gnu.org/licenses/gpl-2.0.html)
% Copyright: ETH Zurich, 2014
% Author: Romano Cicchetti

%params
dataset = 'thun';
households = [1,2,3,4,5,6];
evalDays_type = 'completeSM-first90';
granularity = 1;

power_pct = {};
existing = {};
for h = 1:length(households)
    house = households(h);
    path_to_evalDays = strcat(pwd, '/input/evaluation_days/', dataset, '/', evalDays_type, '/', num2str(house, '%02d'), '.mat');
    load(path_to_evalDays); % evalDays

    % total consumption
    smartmeter_consumption = read_smartmeter_data(dataset, house, evalDays, granularity, 'powerallphases');
    missing_values_idx_sm = smartmeter_consumption == -1;
    total_consumption = sum(smartmeter_consumption(~missing_values_idx_sm));

    % consumption of each appliance
    appliances = findAppliances(house, 'eco');
    for appliance = appliances
       plug_consumption = read_plug_data(dataset, house, appliance, evalDays, granularity);
       missing_values_idx_plug = plug_consumption == -1;
       sum_plug_consumption = sum(plug_consumption(~missing_values_idx_plug));
       ratio = nnz(~missing_values_idx_sm) / nnz(~missing_values_idx_plug);
       extrapolated_plug_consumption = ratio*sum_plug_consumption;
       power_pct{appliance, h} = strcat(num2str(100*extrapolated_plug_consumption / total_consumption, '%.0f'), ' \%');
       existing{appliance, h} = strcat(num2str(100*nnz(~missing_values_idx_plug) / nnz(~missing_values_idx_sm), '%.0f'), ' \%');
    end

    %power_pct{18,h} = sum(power_pct(~isnan(power_pct(:,h)),h));
end

rowLabels = {'Fridge', 'Freezer', 'Microwave', 'Dishwasher', 'Entertainment',...
        'Water kettle', 'Stove', 'Coffee machine', 'Washing machine', 'Dryer', 'Lamp', 'PC', 'Laptop', 'TV', 'Stereo', 'Tablet', 'Router'};
columnLabels = {'House 1', 'House 2', 'House 3', 'House 4', 'House 5', 'House 6'};
matrix2latex(power_pct, 'dataset.tex', 'rowLabels', rowLabels, 'columnLabels', columnLabels, 'format', '%.0f');
matrix2latex(existing, 'dataset2.tex', 'rowLabels', rowLabels, 'columnLabels', columnLabels, 'format', '%0.2f');


 function [Gauss,terms,logp,comptime] = laplacedegenerate_ep(y,X,K,varargin)

% specialized version of our fast variant of
% Expectation Propagation for Logistic Regression (2 classes) with a (correlated) Laplace prior
% in the degenerate case, i.e., when the number of features >> number of samples
% (also works when number of samples > number of features, but then less stable!!!)
%
% input:
% labels    = an N x 1 vector of class labels [1,2]
% examples  = an N x M matrix of input data
% K         = the prior precision matrix of size M x M
%
% Note: the bias term should be explicitly added to K and the examples!
%
% regression parameters refers to betas
% auxiliary variables refers to u and v whose precision matrix auxK = inv(Lambda) couples the features.
%
% we have a precision matrix of the form
%
% | K_beta         |
% |        K_u     |
% |            K_v |
%
% priorGauss: struct with fields
%
%     hatK       diagonal of precision matrix (number of samples x 1);
%                   (initially zero)
%     diagK      diagonal of precision matrix (number of features x 1)
%                   (initially zero)
%
%     precision matrix of regression parameters K_beta = A' hatK A + diagK
%
%     h          canonical mean (number of features x 1)
%                   (initially zero)
%
%     auxK       precision matrix of auxiliary variables (number of features x number of features; sparse)
%                   (contains the covariance structure of interest)
%
%     A          feature matrix (number of samples x number of features)
%
% terms: struct with fields
%
%     hatK       as priorGauss
%     diagK      as priorGauss
%     hath       canonical mean (number of samples x 1)
%     h          canonical mean (number of features x 1)
%                                canonical mean of regression parameters = h + A' hath
%     auxK       as priorGauss, but then only diagonal elements (number of features x 1)
%
% opt: struct with fields (all optional, defaults in brackets)
%
%     maxstepsize   maximum step size [1]
%     fraction      fraction or power for fractional/power EP [1]
%     niter         maximum number of iterations [100]
%     tol           convergence criterion [1e-5]
%     nweights      number of points for numerical integration [20]
%     temperature   temperature for simulated annealing [1]
%     verbose       give output during run [1]
%
% Gauss: struct with fields as in priorGauss plus
%
%     hatB       diagonal of projected covariance (number of samples x 1)
%                                                               hatB = A Covariance A'
%     hatn       projected mean (number of samples x 1)
%                                                               hatn = A m
%     diagC      diagonal of covariance matrix of regression parameters (number of features x 1)
%     auxC       diagonal of covariance matrix of auxiliary variables (number of features x 1)
%
%
% logp: estimated marginal loglikelihood (implementation doubtful...)
%
% comptime: computation time

%% initialization

% parse opt
opt = [];
for i=1:2:length(varargin)
  opt.(varargin{i}) = varargin{i+1};
end

if ~isfield(opt,'maxstepsize'), opt.maxstepsize = 1;    end
if ~isfield(opt,'fraction'),    opt.fraction    = 0.95; end
if ~isfield(opt,'niter'),       opt.niter       = 1000; end
if ~isfield(opt,'tol'),         opt.tol         = 1e-5; end
if ~isfield(opt,'nweights'),    opt.nweights    = 50;   end
if ~isfield(opt,'temperature'), opt.temperature = 1;    end
if ~isfield(opt,'lambda'),      opt.lambda      = 1;    end
if ~isfield(opt,'verbose'),     opt.verbose      = 1;    end

if opt.verbose
  fprintf('starting EP\n');
end

tic

[nsamples,nfeatures] = size(X);

%% create priorGauss and terms

A = X.*repmat(y,1,nfeatures);

% construct Gaussian representation
priorGauss.A     = A;
priorGauss.hatK  = zeros(nsamples,1);
priorGauss.h     = zeros(nfeatures,1);
priorGauss.diagK = zeros(nfeatures,1);
priorGauss.auxK  = K;

% compute additional terms for the EP free energy
[cholK,dummy,S]  = chol(K,'lower');
LogPriorRestAux2 = (2*sum(log(full(diag(cholK))))); % -nfeatures*log(2*pi)); % redundant MvG

% construct term representation
terms.hatK  = ones(nsamples,1)/10;
terms.hath  = zeros(nsamples,1);
terms.diagK = ones(nfeatures,1)./(10*opt.lambda);
terms.auxK  = zeros(nfeatures,1);
terms.h     = zeros(nfeatures,1);

%% precompute points and weights for numerical integration

[xhermite,whermite] = gausshermite(opt.nweights);
xhermite = xhermite(:);       % nhermite x 1
whermite = whermite(:);       % nhermite x 1

[xlaguerre,wlaguerre] = gausslaguerre(opt.nweights);
xlaguerre = xlaguerre(:);     % nlaguerre x 1
wlaguerre = wlaguerre(:);     % nlaguerre x 1

% divide all canonical parameters by the temperature
if opt.temperature ~= 1,
   [priorGauss,terms] = correct_for_temperature(priorGauss,terms,opt.temperature);
end

%% build initial Gauss

% add terms to prior
Gauss        = update_Gauss(priorGauss,terms);
Gauss        = canonical_to_moments(Gauss);
[myGauss,ok] = project_all(Gauss,terms,opt.fraction);
if ~ok,
   error('improper cavity distributions\n');
end

prior = Gauss; % save prior Gauss

%% enter the iterations

logp     = 0;
logpold  = 2*opt.tol;
change   = 0;
teller   = 0;
stepsize = opt.maxstepsize;

while abs(logp-logpold) > stepsize*opt.tol && teller < opt.niter,

   teller    = teller+1;
   logpold   = logp;
   oldchange = change;

   % compute the new term approximation by applying an adf update on all the cavity approximations
   [fullterms,logreglogz,crosslogz] = ...
      adfupdate_all(myGauss,terms,xhermite,whermite,xlaguerre,wlaguerre,opt.fraction,opt.temperature);

   ok  = 0;
   ok1 = 1;
   ok2 = 1;

   while ~ok,

      % try to replace the old term approximations by the new term approximations and check whether the new Gauss is still fine

     [newGauss,newterms,ok1] = try_update(Gauss,fullterms,terms,stepsize);
     %[newGauss,newterms,ok1] = try_update(Gauss,fullterms,terms,0.5);

      if ok1,
         % compute all cavity approximations needed for the next EP updates and check whether they are all fine
         [newGauss,myGauss,logZappx,ok2] = try_project(newGauss,newterms,opt.fraction);
      end

      ok = (ok1 & ok2);
      if ok,    % accept

        terms = newterms;
        Gauss = newGauss;
        stepsize = min(opt.maxstepsize,stepsize*1.9);

      else  % try with smaller stepsize

        stepsize = stepsize/2;
         if ~ok1,
            fprintf('improper full covariance: lowering stepsize to %g\n',stepsize');
         elseif ~ok2,
            fprintf('improper cavity covariance: lowering stepsize to %g\n',stepsize');
         end
         if stepsize < 1e-10,
            warning('Cannot find an update that leads to proper cavity approximations');
            teller = opt.niter;
            break;
         end

      end

   end

   % compute marginal moments

   if ok,

      % compute marginal loglikelihood

      %logp = sum(logz) + sum(crosslogz) + logdet/2 + contribQ;
%      CorrTermCross1 = (myGauss.m.^2)./myGauss.diagC - (newGauss.m.^2)./newGauss.diagC + log(myGauss.diagC/newGauss.diagC);
%	   CorrTermCross2 = log(myGauss.auxC/newGauss.auxC);
%      CorrTerm  = 0.5*CorrTermCross1 + 0.5*(CorrTermCross2 + CorrTermCross2);

      logp      = (sum(crosslogz) + sum(logreglogz))./opt.fraction + logZappx + LogPriorRestAux2;

      if opt.verbose
        fprintf('%d: %g (stepsize: %g)\n',teller,logp,stepsize);
      end

      % check whether marginal loglikelihood is going up and down, if so, lower stepsize

      change = logp-logpold;
      if change*oldchange < 0,   % possibly cycling
         stepsize = stepsize/2;
      end
      oldchange = change;
   end

end

comptime = toc;

if opt.verbose
  fprintf('EP finished in %s seconds\n',num2str(comptime));
end

Gauss.prior = prior;

%%% END MAIN

%%%%%%%%%
%
% compute the cavity approximations that result when subtracting a fraction of the term approximations

function [myGauss,ok] = project_all(Gauss,terms,fraction)

if nargin < 3,
   fraction = 1;
end

% take out and project in moment form

% (1) regression parameters

[myGauss.hatB,myGauss.hatn] = ...
   rank_one_update(Gauss.hatB,Gauss.hatn,-fraction*terms.hatK,-fraction*terms.hath);

% (2) cross terms between regression parameters and auxiliary parameters

[myGauss.diagC,myGauss.m] = ...
   rank_one_update(Gauss.diagC,Gauss.m,-fraction*terms.diagK,-fraction*terms.h);

 myGauss.auxC = rank_one_update(Gauss.auxC,[],-fraction*terms.auxK);

% check whether all precision matrices are strictly positive definite

if nargout > 1,
   ok = (all(myGauss.hatB > 0) & all(myGauss.diagC > 0) & all(myGauss.auxC > 0));
end

%%%%%%%%%
%
% compute the new term approximation by applying an adf update on all the cavity approximations

function [fullterms,logreglogz,crosslogz] = ...
   adfupdate_all(myGauss,terms,xhermite,whermite,xlaguerre,wlaguerre,fraction,temperature)

if nargin < 8,
   temperature = 1;
end
if nargin < 7,
   fraction = 1;
end

%% (1) regression parameters

oldm     = myGauss.hatn;
oldC     = myGauss.hatB;
sqrtC    = sqrt(oldC);
nsamples = length(oldm);
nhermite = length(whermite);

% translate and scale the sample points to get the correct mean and variance

x = repmat(oldm,1,nhermite) + sqrtC*xhermite';

% compute the terms at the sample points

g = logist(x);   % returns - log (1 + exp(-x)) with special attention for very small and very large x

% correct for fraction and temperature and incorporate the sample weights

g           = fraction*g/temperature + log(repmat(whermite',nsamples,1));
maxg        = max(g,[],2);
g           = g-repmat(maxg,1,nhermite);
expg        = exp(g);
denominator = sum(expg,2);
neww        = expg./repmat(denominator,1,nhermite);

% compute the moments

Ex   = sum(x.*neww,2);
Exx  = sum(x.^2.*neww,2);
newm = Ex;
newC = Exx-Ex.^2;

% derive the term approximation from the change in mean and variance

[fullterms.hatK,fullterms.hath,logzextra] = compute_termproxy(newC,newm,oldC,oldm,fraction);

% contributions to marginal loglikelihood
logreglogz = maxg + log(denominator) + logzextra;

%% (2) cross terms between regression parameters and auxiliary variables

oldm      = myGauss.m;
oldC      = myGauss.diagC;
oldlambda = myGauss.auxC;
nfeatures = length(oldm);
nlaguerre = length(wlaguerre);

% this part heavily relies on the accompanying note
% basic idea:
% - the cavity approximation on U is an exponential distribution
% - we have analytical formulas for the moments of x conditioned upon U
% - marginal moments can then be computed through numerical integration with Gauss-Laguerre

% translate and scale the sample points to get the correct mean

U  = 2*oldlambda*xlaguerre';     % nfeatures x nlaguerre

mm = repmat(oldm,1,nlaguerre);
CC = repmat(oldC,1,nlaguerre);

% compute the partition function (integral over x) given U and turn this into weights required for computing the marginal moments

g           = (1-fraction)*log(U)/2 - fraction*mm.^2./(U + fraction*CC)/2 - log(U+ fraction*CC)/2;

g           = bsxfun(@plus,g,log(wlaguerre'));
maxg        = max(g,[],2);
g           = bsxfun(@minus,g,maxg);
expg        = exp(g);

denominator = sum(expg,2);

neww        = bsxfun(@rdivide,expg,denominator);

% compute the marginal moments through numerical integration

ExgU = mm.*U./(U + fraction*CC);
Ex   = sum(ExgU.*neww,2);

ExxgU = ExgU.^2 + CC.*U./(U+fraction*CC);
Exx   = sum(ExxgU.*neww,2);
EU    = sum(U.*neww,2);

newm      = Ex;
newC      = Exx-Ex.^2;
newlambda = EU/2;

% derive the term approximation from the change in mean and variance

[fullterms.diagK,fullterms.h,logzextra1] = compute_termproxy(newC,newm,oldC,oldm,fraction);
[fullterms.auxK,dummy,logzextra2] = compute_termproxy(newlambda,zeros(nfeatures,1),oldlambda,zeros(nfeatures,1),fraction);

crosslogz = maxg + log(denominator) + logzextra1 + 2*logzextra2;
% multiplied the last term by two

%%%%%%%%%%
%
% compute the moments corresponding to the canonical parameters

function [Gauss,logp] = canonical_to_moments(Gauss)

[nsamples,nfeatures] = size(Gauss.A);

%% (1) regression parameters

if nsamples > nfeatures,   % in the non-degenerate case, this direct route is more stable and faster

   scaledA     = Gauss.A.*(repmat(Gauss.hatK,1,nfeatures));
   K           = Gauss.A'*scaledA + diag(Gauss.diagK);
   [C,logdet1] = invert_chol(K);
   Gauss.m     = C*Gauss.h;
   Gauss.hatB  = zeros(nsamples,1);          % only need diagonal
   for k=1:nsamples,
      Gauss.hatB(k) = Gauss.A(k,:)*C*Gauss.A(k,:)';
   end
   Gauss.diagC = diag(C);
   Gauss.hatn  = Gauss.A*Gauss.m;

else

   % this part heavily relies on the appendix of the accompanying note
   % basic idea:
   % - the precision matrix K is of the form A' hatK A + diagK, where both hatK and diagK are diagonal matrices
   % - apply Woodbury's formula to replace inverting an (nfeat x nfeat) matrix by an (nsample x nsample) alternative
   % - projections of the covariance matrix and the mean onto the feature matrix then follow immediately

   scaledA     = bsxfun(@rdivide,Gauss.A,Gauss.diagK');
   W           = Gauss.A*scaledA';
   W           = (W + W')/2;    % make symmetric
   [Q,logdet1] = invert_chol(diag(1./Gauss.hatK) + W);

   Gauss.hatB = zeros(nsamples,1);
   for k=1:nsamples,
      Gauss.hatB(k) = W(k,k) - W(k,:)*Q*W(:,k);
   end

   Gauss.m     = Gauss.h./Gauss.diagK - scaledA'*(Q*(scaledA*Gauss.h));
   Gauss.hatn  = Gauss.A*Gauss.m;

   Gauss.diagC = 1./Gauss.diagK;

%     for i=1:nfeatures,
%        Gauss.diagC(i) = Gauss.diagC(i) - scaledA(:,i)'*Q*scaledA(:,i);
%     end

   % adriana's recipe
   z = scaledA' * Q; for i=1:size(z), Gauss.diagC(i) = Gauss.diagC(i) - z(i,:) * scaledA(:,i); end

   logdet1 = logdet1 + sum(log(Gauss.diagK)) + sum(log(Gauss.hatK));

end

% compute quadratic term (BC)

qterm = sum(Gauss.m .* Gauss.diagK .* Gauss.m); % = m' * diagK * m
qterm = qterm + sum(Gauss.hatn .* Gauss.hatK .* Gauss.hatn);

logp1 = 0.5*(qterm - logdet1);

%% (2) auxiliary variables; i.e., wrt scale mixture representation of
% Laplace prior

% this is (by far) the most expensive step when nsamples << nfeatures
% and the precision matrix of the auxiliary variables is non-diagonal

[auxC,logdet2] = invert_chol(Gauss.auxK); % only need diagonal terms
Gauss.auxC     = full(diag(auxC));        % turn into full vector
logp2          = 0.5*( - logdet2);
logp           = logp1 + 2*logp2;

%%%%%%%%%%
%
% take out the old term proxies and add the new termproxies and check whether the resulting Gaussian is still normalizable

function [newGauss,newterms,ok] = try_update(Gauss,fullterms,terms,stepsize)

if nargin < 4,
   stepsize = 1;
end

% take out the old term proxies

newGauss = update_Gauss(Gauss,terms,-1);

% compute the new term proxies as a weighted combi of the old ones and the "full" (stepsize 1) term proxies

newterms = combine_terms(fullterms,terms,stepsize);

% add the new term proxies

newGauss = update_Gauss(newGauss,newterms,1);

[L,check,dummy] = chol(newGauss.auxK,'lower');   % check whether full covariance matrix is ok
                % note that this is bit inefficient, since we redo the Cholesky later when everything is fine

ok = (check == 0 & all(newGauss.hatK > 0) & all(newGauss.diagK > 0));  % perhaps a bit too strong???

%%%%%%%%%%%%
%
% compute the moment form of the current Gauss and all cavity approximations and check whether they are fine

function [Gauss,myGauss,logdet,ok] = try_project(Gauss,terms,fraction)

if nargin < 3,
   fraction = 1;
end

[Gauss,logdet] = canonical_to_moments(Gauss);
[myGauss,ok]   = project_all(Gauss,terms,fraction);

%%%%%%%%%%
%
% if we use a temperature < 1, to get closer to the MAP solution, we have to change the prior and initial term proxies accordingly

function [Gauss,terms] = correct_for_temperature(Gauss,terms,temperature)

% note: choose temperature small to implement MAP-like behavior

Gauss.hatK  = Gauss.hatK/temperature;
Gauss.h     = Gauss.h/temperature;
Gauss.auxK  = Gauss.auxK/temperature;
Gauss.diagK = Gauss.diagK/temperature;

terms.hatK  = terms.hatK/temperature;
terms.hath  = terms.hath/temperature;
terms.diagK = terms.diagK/temperature;
terms.auxK  = terms.auxK/temperature;
terms.h     = terms.h/temperature;

%%%%%%%%%%
%
% invert a positive definite matrix using Cholesky factorization

function [invA,logdet] = invert_chol(A)

if issparse(A)

  if 0 % matlab version; slower but useful in case of mex problems

    [L,dummy,S] = chol(sparse(A),'lower');   % now A = S*(L*L')*S' and (L*L') = S'*A*S

    n           = length(L);
    invdiagL2   = 1./spdiags(L,0).^2;

    invA = A;
    for i=n:-1:1,
      I      = i+find(L(i+1:n,i));
      invA(I,i) = -(invA(I,I)*L(I,i))/L(i,i);
      invA(i,I) = invA(I,i)';
      invA(i,i) = invdiagL2(i) - (invA(i,I)*L(I,i))/L(i,i);
    end

    invA = S*invA*S';

  else

    [L,dummy,S] = chol(sparse(A),'lower');   % now A = S*(L*L')*S' and (L*L') = S'*A*S

    if dummy
      error('matrix is not p.d.');
    end

    invA = fastinvc(L);
    invA = S*invA*S';
  end

else

  [L,dummy] = chol(A,'lower');

  if dummy
    error('matrix is not p.d.');
  end

  invA = inv(A);

end

if nargout > 1,
  logdet = 2*sum(log(full(diag(L))));
end

%%%%%%%%%%
%
% compute the term proxy when [oldC,oldm] changes to [newC,newm]

function [K,h,logz] = compute_termproxy(newC,newm,oldC,oldm,fraction)

if nargin < 5,
   fraction = 1;
end

K    = (1./newC - 1./oldC)/fraction;
h    = (newm./newC - oldm./oldC)/fraction;
logz = oldm.^2./oldC/2 - newm.^2./newC/2 + log(full(oldC./newC))/2 ;

%%%%%%%%%%
%
% Sherman-Morrison formula to compute the change from [oldC,oldm] to [newC,newm] when we add [K,h] to the corresponding canonical parameters

function [newC,newm] = rank_one_update(oldC,oldm,K,h)

dummy         = K.*oldC;
oneminusdelta = 1./(1+dummy);
newC          = oneminusdelta.*oldC;

if nargout > 1,
   newm = oneminusdelta.*(oldm + h.*oldC);
end

%%%%%%%%%%%
%
% general procedure for a weighted combi of the fields of two structures

function terms = combine_terms(terms1,terms2,stepsize)

names1 = fieldnames(terms1);
names2 = fieldnames(terms2);
names  = intersect(names1,names2);

terms = struct;
for i=1:length(names)
  terms.(names{i}) = stepsize*terms1.(names{i}) + (1-stepsize)*terms2.(names{i});
end

%%%%%%%%%%%
%
% updates the Gaussian representation with new term proxies

function Gauss = update_Gauss(Gauss,terms,const)

if nargin < 3,
   const = 1;
end

Gauss.h     = Gauss.h     + const*Gauss.A'*terms.hath + const*terms.h;
Gauss.hatK  = Gauss.hatK  + const*terms.hatK;
Gauss.diagK = Gauss.diagK + const*terms.diagK;

% get diagonal elements
diagidx             = 1:(size(Gauss.auxK,1)+1):numel(Gauss.auxK);
Gauss.auxK(diagidx) = Gauss.auxK(diagidx) + const*terms.auxK';


classdef TREE3 < PROBLEM
% <multi> <real> <large/none> <constrained> <expensive/none>
% The time-varying ratio error estimation problem
% T --- 1000 --- Length of data (related to the number of variables)

%------------------------------- Reference --------------------------------
% C. He, R. Cheng, C. Zhang, Y. Tian, Q. Chen, and X. Yao, Evolutionary
% large-scale multiobjective optimization for ratio error estimation of
% voltage transformers, IEEE Transactions on Evolutionary Computation,
% 2020, 24(5): 868-881.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties(Access = private)
        Data;   % Dataset
        Mean;   % Mean values of the dataset
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            % Load data
            T = obj.ParameterSet(1000);
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'Dataset_TREE.mat'),'Dataset');
            obj.Data = Dataset.TREE3(1:min(max(T,10),end),:);
            % Set the numbers of objectives and decision variables
            K = 6;
            obj.M = 2;
            obj.D = T*K;
            % Set the upper and lower boundaries
            Lower    = zeros(T,K);
            Upper    = zeros(T,K);
            obj.Mean = zeros(T,K);
            for i = 1 : 3
                Lower(:,i)      = min(obj.Data(:,i:3:end*0.5),[],2)/1.01;
                Upper(:,i)      = max(obj.Data(:,i:3:end*0.5),[],2)/0.99;
                Lower(:,i+3)    = min(obj.Data(:,end*0.5+i:3:end),[],2)/1.01;
                Upper(:,i+3)    = max(obj.Data(:,end*0.5+i:3:end),[],2)/0.99;
                obj.Mean(:,i)   = mean(obj.Data(:,i:3:end*0.5),2);
                obj.Mean(:,i+3) = mean(obj.Data(:,end*0.5+i:3:end),2);
            end
            % The decision variables are the offset of the mean values of
            % the dataset
            obj.lower    = reshape(Lower-obj.Mean,1,[]);
            obj.upper    = reshape(Upper-obj.Mean,1,[]);
            obj.encoding = ones(1,obj.D);
        end
        %% Generate initial solutions
        function Population = Initialization(obj,N)
            if nargin < 2; N = obj.N; end
            PopDec = obj.Mean(:)'.*(rand(N,obj.D)*0.008-0.004);
            PopDec = min(max(PopDec,repmat(obj.lower,N,1)),repmat(obj.upper,N,1));
            Population = obj.Evaluation(PopDec);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            N      = size(PopDec,1);
            KP     = size(obj.Data,2);
            [T,K]  = size(obj.Mean);
            PopDec = reshape(PopDec,N,T,K) + repmat(reshape(obj.Mean,1,T,K),N,1,1);
            PopObj = zeros(N,obj.M);
            % First objective
            eA1 = abs(repmat(reshape(obj.Data(:,1:end*0.5),1,T,KP/2),N,1,1)./repmat(PopDec(:,:,1:3),1,1,KP/6)-1);
            eA2 = abs(repmat(reshape(obj.Data(:,end*0.5+1:end),1,T,KP/2),N,1,1)./repmat(PopDec(:,:,4:6),1,1,KP/6)-1);
            eA  = cat(3,eA1,eA2);
            PopObj(:,1) = sum(sum(eA,3),2);
            % Second objective
            Delta = std(eA(:,2:end,:)-eA(:,1:end-1,:),0,2);
            PopObj(:,2) = sum(reshape(Delta,N,[]),2);
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,PopDec)
            N      = size(PopDec,1);
            [T,K]  = size(obj.Mean);
            PopDec = reshape(PopDec,N,T,K) + repmat(reshape(obj.Mean,1,T,K),N,1,1);
            PopCon = TREE_CalCon(PopDec,2);
        end
        %% Generate a point for hypervolume calculation
        function R = GetOptimum(obj,~)
            X = zeros(1,obj.D);
            X(1:2:end) = obj.lower(1:2:end);
            X(2:2:end) = obj.upper(2:2:end);
            R = obj.CalObj(X);
        end
    end
end


function pred = ml_predictsvmperf(trials, model)
% Prediction function for the Support Vector Machine (SVMperf).
% Prediction = ml_predictsvmperf(Trials, Model)
%
% In:
%   Trials  : the data a matrix, as in ml_predict
%
%   Model   : predictive model as produced by ml_trainsvmlight
%
% Out:
%   Prediction  : discrete probability distribution, formatted as
%                 {'disc' [NxC] [Cx1]}, with element #2 being the per-class probability and
%                 element #3 the original target values per class
%                 thus, the expected target values are Prediction{2}*Prediction{3}
%
% See also:
%   ml_trainsvmperf
%
%                           Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
%                           2010-04-04

if isfield(model,'voted')
    pred = ml_predictvote(trials,model);
else
    trials = hlp_applyscaling(trials,model.sc_info);
    raw_pred = min(+1,max(-1,svmperfclassify(trials, ones(size(trials,1),1), model, '-v 0')));
    pred = {'disc', [(1-raw_pred)/2 1-(1-raw_pred)/2], model.classes};
end


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

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

% No reason to try to do factor tracking here
x = flush(x);

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 hf_out = lhs_operation(hf, samplesf, reg_filter, sample_weights, feature_reg)

% This is the left-hand-side operation in Conjugate Gradient

% size of the padding
num_features = length(hf);
output_sz = [size(hf{1},1), 2*size(hf{1},2)-1];
pad_sz = cellfun(@(hf) (output_sz - [size(hf,1), 2*size(hf,2)-1]) / 2, hf, 'uniformoutput',false);

% Compute the operation corresponding to the data term in the optimization
% (blockwise matrix multiplications)
%implements: A' diag(sample_weights) A f

% sum over all features in each block
sh_cell = cellfun(@(hf,samplesf) mtimesx(samplesf, permute(hf, [3 4 1 2]), 'speed'), hf, samplesf, 'uniformoutput', false);

% sum over all feature blocks
sh = sh_cell{1};    % assumes the feature with the highest resolution is first
for k = 2:num_features
    sh(:,1,1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end) = ...
        sh(:,1,1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end) + sh_cell{k};
end

% weight all the samples
sh = bsxfun(@times,sample_weights,sh);

% multiply with the transpose
hf_out = cellfun(@(samplesf,pad_sz) permute(conj(mtimesx(sh(:,1,1+pad_sz(1):end-pad_sz(1), 1+pad_sz(2):end), 'C', samplesf, 'speed')), [3 4 2 1]), ...
    samplesf, pad_sz, 'uniformoutput', false);

% compute the operation corresponding to the regularization term (convolve
% each feature dimension with the DFT of w, and the tramsposed operation)
% add the regularization part

reg_pad = cellfun(@(hf, reg_filter) min(size(reg_filter,2)-1, size(hf,2)-1), hf, reg_filter, 'uniformoutput', false);

% add part needed for convolution
hf_conv = cellfun(@(hf,reg_pad) cat(2, hf, conj(rot90(hf(:, end-reg_pad:end-1, :), 2))), hf, reg_pad, 'uniformoutput', false);

% do first convolution
hf_conv = cellfun(@(hf_conv, reg_filter) convn(hf_conv, reg_filter), hf_conv, reg_filter, 'uniformoutput', false);

% do final convolution and put toghether result
hf_out = cellfun(@(hf_conv, hf_data, reg_filter, hf, reg_pad, feature_reg) hf_data + convn(hf_conv(:,1:end-reg_pad,:), reg_filter, 'valid') + feature_reg * hf, ...
    hf_conv, hf_out, reg_filter, hf, reg_pad, feature_reg, 'uniformoutput', false);

end


filename='Tests_Triangle_Linear';
ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance','perimeterInterior'};
weights = [1 1];
constraint = {'volumeConstraint'};
%optimizer = 'DualNestedInPrimal';
optimizer = 'AlternatingPrimalDual';

optimizerUnconstrained = 'SLERP';
incrementFactor = 1;
designVariable = 'LevelSet';
filterType = 'PDE';

nsteps = 1;
Vfrac_final = 0.3;
optimality_final =1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-2;
constr_initial = 1e-3;

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

plotting = false;
printing = false;
monitoring = false;

maxiter = 2;


function f_x = ParFor2(in1)
%PARFOR2
%    F_X = PARFOR2(IN1)

%    This function was generated by the Symbolic Math Toolbox version 8.2.
%    20-Sep-2019 09:35:34

u = in1(:,1);
ux = in1(:,4);
uxx = in1(:,5);
uxxx = in1(:,6);
f_x = (u.*1.034234549167718e16+ux.*6.60584588804096e15+uxx.*1.215161942676603e17-uxxx.*4.609471812108288e15-u.*ux.*5.122148099342336e15+u.*uxx.*5.033451929390285e16+u.*uxxx.*5.000975400599552e15-2.603066969792512e15)./(u.*3.126164642771763e16+4.756339916996608e16);


function regresdiagplot3d(sdist,odist,rdist,cutoffsd,cutoffod,cutoffrd,k,class,multi,labsd,labod,labresd,labels)

%REGRESDIAGPLOT3D is a 3D-outlier map which visualizes the orthogonal distance,
% the score distance and the residual distance calculated in a PCR or PLSR analysis.
%
% I/O: regresdiagplot3D(sdist,odist,rdist,cutoffsd,cutoffrd,k,class,labsd,labod,labresd,labels)
%
% Example: out=rpcr(X,Y);
%          regresdiagplot3d(out.sd,out.od,out.resd,out.cutoff.sd,cutoff.od,...
%          out.cutoff.resd,out.k,out.class,1,out.labsd,out.labod,out.labresd,0)
%
% Uses function: putlabel
%
% Created on by S.Verboven
% Last revision: 09/04/2004
%

%INITIALIZATION%
if nargin<13
    labels=0;
end
if nargin<10
    labsd=3;
    labod=3;
    labresd=3;
    labels=0;
end
if nargin==10
    labsd=3
    labels=0;
    labresd=3;
    labod=3;
end
if nargin==11
    labels=0;
    labresd=3;
    labod=3;
end
if nargin==12
    labels=0;
    labresd=3;
end
if nargin<9
    error('A required input variable is missing!')
end

%all LTS-analysis in RPCR are intercept included!!!
% if ask==2 %multivariate analysis
%     %residual distances
%
% else  %univariate analysis
%     %standardized residuals
%     cutoffz=2.5;
% end
cutoffxx=cutoffsd;
cutoffxy=cutoffsd;
cutoffyy=cutoffod;
cutoffyx=cutoffod;
cutoffz=cutoffrd;
x=sdist;
y=odist;
z=rdist;

%%%%%%%MAIN FUNCTION%%%%%%%
set(gcf,'Name', '3D-Outlier map (regression)', 'NumberTitle', 'off')%,'Renderer','OpenGL');

%%%%%%%%Odist=0 not yet included in this standalone!!!!!!!!! included in
%%%%%%%%makeplot function!!!!
plot3(x,y,z,'ko','markerfacecolor',[0.75 0.75 0.75])

hold on
axhandle=gca;
ylen=get(axhandle, 'Ylim');
xlen=get(axhandle,'Xlim');
zlen=get(axhandle,'Zlim');
xrange=xlen(2)-xlen(1);
upLimx=max(cutoffxx,xlen(2))+xrange*0.1;
lowLimx=xlen(1)-xrange*0.1;
yrange=ylen(2)-ylen(1);
upLimy=ylen(2)+yrange*0.1;
lowLimy=ylen(1)-yrange*0.1;
zrange=zlen(2)-zlen(1);
upLimz=max(cutoffz,zlen(2))+zrange*0.1;
if cutoffz==2.5
    lowLimz=min(-2.5,zlen(1))-zrange*0.1;
else
    lowLimz=min(cutoffz,zlen(1))-zrange*0.1;
end

%axis square;
set(gca, 'Xlim',[lowLimx upLimx],'Ylim',[lowLimy upLimy],'Zlim',[lowLimz,upLimz])
hold on

xlabel('Score distance')
ylabel('Orthogonal distance')
if cutoffz~=2.5
    zlabel('Residual distance')
else
    zlabel('Standardized Residual')
end
grid on

%in XY-space
%red plane "vertical" on x axis
oppy=[upLimy:-0.01:lowLimy];
n=length(oppy);
h=(upLimz-lowLimz)/n;
oppz=[lowLimz:h:upLimz];

X1=cutoffxx*ones(n);
Y1=repmat(oppy,n,1);
Z1=repmat([oppz(1:n-1)'; upLimz],1,n);
surf(X1,Y1,Z1,'edgecolor','none','facecolor','r')
alpha(.3)

%blue "horizontal" planes orthogonal on z-axis
%in ZX and ZY-space
oppx=[lowLimx:0.05:upLimx];
n=length(oppx);
X2=repmat(oppx,n,1);
h=(upLimy-lowLimy)/n;
oppy=[lowLimy:h:upLimy];
Y2=repmat(oppy(1:n)',1,n);
Z2=cutoffz*ones(n);
surf(X2,Y2,Z2,'edgecolor','none','facecolor','b')
alpha(0.3)

% in XY-space
%green plane "vertical" on y axis
oppx=[upLimx:-0.01:lowLimx];
n=length(oppx);
h=(upLimz-lowLimz)/n;
oppz=[lowLimz:h:upLimz];
X1=repmat(oppx,n,1);
Y1=cutoffyy*ones(n);
Z1=repmat([oppz(1:n-1)'; upLimz],1,n);
surf(X1,Y1,Z1,'edgecolor','none','facecolor','g')
alpha(0.3)

if multi==0 %univariate case
    surf(X2,Y2,-Z2,'edgecolor','none','facecolor','b')
    alpha(0.3)
end

if labels~=0
    putlabel(x,y,labels,z,labels)
else
    plotnumbers(x,y,labsd,labod,5,z,labresd)
end
hold off


function preconfun = preconhessiansolve(problem, options)
% Preconditioner based on the inverse Hessian, by solving linear systems.
%
% function preconfun = preconhessiansolve(problem)
% function preconfun = preconhessiansolve(problem, options)
%
% Input:
%
% A Manopt problem structure (already containing the manifold and enough
% information to compute the Hessian of the cost) and an options structure
% (optional, currently ignored). Notice that if the Hessian is not positive
% definite, then its inverse is not positive definite either and this
% preconditioner is not suitable.
%
% If the Hessian cannot be computed on 'problem', a warning is issued. An
% approximation of the Hessian will be used instead, and the present
% preconditioner will attempt to invert that (although it may not be a
% linear operator). If no approximate Hessian is provided either, a generic
% approximation is used. Behavior is unspecified.
%
% Output:
%
% Returns a function handle, encapsulating a generic preconditioner of the
% Hessian based on solving linear systems of the form:
%   Hessian(x)[preconfun(x, xdot)] = xdot,
% where x is the point on the manifold, xdot is the input to the
% preconditioner (a tangent vector) and preconfun(x, xdot) is returned
% (also a tangent vector). The solve may be approximate.
%
% The returned preconfun has this calling pattern:
%
%   function precxdot = preconfun(x, xdot)
%   function precxdot = preconfun(x, xdot, storedb)
%   function precxdot = preconfun(x, xdot, storedb, key)
%
% x is a point on the manifold problem.M, xdot is a tangent vector to that
% manifold at x, storedb is a StoreDB object, and key is the StoreDB key to
% point x.
%
% Usage:
%
% Typically, the user will set problem.M and other fields to define the
% cost, the gradient and the Hessian (typically, problem.cost, problem.grad
% and problem.hess, or problem.egrad and problem.ehess). Then, to use this
% generic purpose Hessian preconditioner:
%
%   problem.precon = preconhessiansolve(problem, options);
%
% Passing that problem structure to the conjugategradients solver
% (which uses preconditioning) configured in steepest descent mode results
% in a type of Riemannian Newton method.
%
% See also: conjugategradients

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, April 9, 2015.
% Contributors:
% Change log:

    % Check availability of the Hessian, or at least of an approximation.
    if ~canGetHessian(problem) && ~canGetApproxHessian(problem)
        % Note: we do not give a warning if an approximate Hessian is
        % explicitly given in the problem description, as in that case the
        % user seems to be aware of the issue.
        warning('manopt:getHessian:approx', ...
               ['No Hessian provided. Using an FD approximation instead.\n' ...
                'To disable this warning: warning(''off'', ''manopt:getHessian:approx'')']);
        problem.approxhess = approxhessianFD(problem);
    end

    % Set local defaults here, and merge with user options, if any.
    localdefaults = struct();
    if ~exist('options', 'var') || isempty(options)
        options = struct();
    end
    options = mergeOptions(localdefaults, options);

    % Build and return the function handle here. This extra construct via
    % funhandle makes it possible to make storedb and key optional.
    preconfun = @funhandle;
    function precxdot = funhandle(x, xdot, storedb, key)
        % Allow omission of the key, and even of storedb.
        if ~exist('key', 'var')
            if ~exist('storedb', 'var')
                storedb = StoreDB();
            end
            key = storedb.getNewKey();
        end
        precxdot = hessiansolvehelper(options, problem, x, xdot, ...
                                      storedb, key);
    end

end

function precxdot = hessiansolvehelper(options, problem, x, xdot, storedb, key)
% This function does the actual work.

    % Exclude the case where xdot is zero
    norm_xdot = problem.M.norm(x, xdot);
    if norm_xdot < eps
        precxdot = problem.M.zerovec(x);
        return;
    end

    % Get a shorthand for the Hessian of the cost on M at x.
    hessian = @(u) getHessian(problem, x, u, storedb, key);

    % Setup an optimization problem on the tangent space to problem.M at x.
    M = problem.M;
    tgtspace = tangentspacefactory(M, x);
    prblm.M = tgtspace;
    prblm.cost = @cost;
    prblm.grad = @grad;
    prblm.hess = @(u, udot) 2*hessian(hessian(udot))/norm_xdot;

    function [f, store] = cost(u, store)
        if ~isfield(store, 'residue')
            Hu = hessian(u);
            store.residue = M.lincomb(x, 1, Hu, -1, xdot);
        end
        f = M.norm(x, store.residue).^2 / norm_xdot;
    end
    function [g, store] = grad(u, store)
        if ~isfield(store, 'residue')
            Hu = hessian(u);
            store.residue = M.lincomb(x, 1, Hu, -1, xdot);
        end
        g = 2 * hessian(store.residue) / norm_xdot;
    end

    % checkgradient(prblm); pause;
    % checkhessian(prblm); pause;

    localdefaults.solver = @trustregions;
    localdefaults.verbosity = 0;
    % Merge local defaults with user options, if any.
    if ~exist('options', 'var') || isempty(options)
        options = struct();
    end
    options = mergeOptions(localdefaults, options);

    % Solve the linear system by solving the optimization problem.
    precxdot = manoptsolve(prblm, M.zerovec(), options);

end


function a = sec(a)
%SEC          Gradient secant sec(a)
%

% written  10/16/98     S.M. Rump
% modified 10/14/00     S.M. Rump  use Tony's trick
% modified 03/22/04     S.M. Rump  improved performance
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/08/08     S.M. Rump  improved sparse multiplication: not using intval data type
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');

  ax = tan(a.x(:));
  a.x = sec(a.x);
  ax = a.x(:) .* ax;
  if issparse(a.dx)
    sizeax = size(a.dx,1);
    [ia,ja,sa] = find(a.dx);
    % take care of scalar a.x: cures Matlab V6.0 bug
    % a=7; i=[1 1]; x=a(i), b=sparse(a); y=b(i)  yields row vector x but column vector y
    ax = ax(ia);
    if isa(a.x,'intval')
      adx = times(ax(:),sa(:),0);
      if adx.complex
        a.dx = intval( sparse(ia,ja,adx.mid,sizeax,N) , sparse(ia,ja,adx.rad,sizeax,N) , 'midrad' );
       else
        a.dx = intval( sparse(ia,ja,adx.inf,sizeax,N) , sparse(ia,ja,adx.sup,sizeax,N) , 'infsup' );
      end
    else
      a.dx = sparse(ia,ja,ax(:).*sa(:),sizeax,N);
    end
  else
    a.dx = a.dx .* ax(:,ones(1,N));
  end

  if rndold
    setround(rndold)
  end


function [mri] = align_fsaverage2mni(mri)

% ALIGN_FSAVERAGE2MNI performs an affine alignment of the anatomical volume from
% FSAVERAGE towards MNI coordinates. Only the homogeneous transformation matrix is
% modified and the coordsys-field is updated.
%
% Use as
%   mri = align_fsaverage2mni(mri)
% where the first input argument is a FieldTrip MRI-structure.
%
% with fsaverage we mean MNI305
% with mni       we mean MNI152, i.e. the template used in SPM
%
% See http://freesurfer.net/fswiki/CoordinateSystems
%
% See also ALIGN_CTF2ACPC, ALIGN_NEUROMAG2ACPC

fsaverage2mni = [
   0.9975   -0.0073    0.0176   -0.0429
   0.0146    1.0009   -0.0024    1.5496
  -0.0130   -0.0093    0.9971    1.1840
  ];

assert(strcmp(mri.coordsys, 'fsaverage'), 'incorrect input coordinate system ''%s''', mri.coordsys);
mri.transform = fsaverage2mni * mri.transform;
mri.coordsys = 'mni';


% sound2gyro.m
% evaluate effect of sound signal on phone internal gyro
%  publish('sound2gyro.m','doc');

Fs = 200;                   % 200 sps default
[filenam, pathnam] = uigetfile('*.txt', 'get measurements data file');
fid = fopen(filenam,'r');
% 0.0
% 0.000000 -0.044288
% 5046272.000000 -0.044288
% L2:L212 =[txyz] (tinnsec)
% repeat n times
n=20;
f_in = zeros(n,1);
txyz = zeros(200,4,n);
frewind(fid),
i=1;
f_in(i) = str2double(fgetl(fid)); % read a line, 1st is noise freq
while ~isnan(f_in(i))
    linenums = 4;
    cntlines = 1;
    while linenums == 4
        % # of nums per line
        % counting for sampnum
        % loop to read one noise sequence
        tt = fgetl(fid);
        ttnum = str2num(tt);
        linenums = length(ttnum);      %
        if linenums ==4,
            txyz(cntlines,:,i) = ttnum;
            cntlines = cntlines+1;
        end;
    end;
    % get next line chars
    % make numbers
    sampnum = cntlines - 1;             % last was 1 or nan
    i = i+1;
    if isempty(ttnum), f_in(i)=nan; else f_in(i) = ttnum; end;
end

f_in(i:end)=[];             % clean the rest
txyz(:,:,i:end) = [];
n = i-1;                    % last one was NAN
% set  # of columns for plots
if n>5
    pcols = 2;
else
    pcols = 1;
end;
prows = n/pcols;
fclose(fid);

filenam = 'RAFDOCS-#7775044Magn.TXT';

if findstr(filenam,'77750')     % .. 28 | 44
    savetxyz = txyz;
    resamp;     % returns newtxyz, f_in, Fs, nlen
    len = min(nlen);
    txyz = zeros(len,4, length(f_in));
    for kf = 1:n,
        txyz(:,:,kf) = newtxyz{kf}(1:len,:);
    end
    xyz = txyz(:,2:4,:);
    time = squeeze(txyz(:,1,:));
    t = time;
else   %% verify, update ->  len, n
    len = 199;
    % clean
    % orig in nsec, Fs
    % clean
    t = repmat((0:len-1)'/Fs,1,n);
end;

xyzstr = 'XYZ';

xyz = txyz(1:len,2:4,:);      % 199x3xn
% x10 = squeeze(txyz(1:len,2,:));
% y10 = squeeze(txyz(1:len,3,:));
% z10 = squeeze(txyz(1:len,4,:));

for d = 1:3            % dimensions
    figure
    mn = min(min(squeeze(xyz(:,d,:))));
    mx = max(max(squeeze(xyz(:,d,:))));
    for i=1:n
        subplot(prows, pcols,i)
        curr = xyz(:,d,i);
        plot(t(:,i),curr,'.-')
        axis([0 t(end,i) mn mx]);
        title([ xyzstr(d) '-meas. for freq = ' num2str(f_in(i)) ' Hz'])
        xlabel('time [s]'), grid on
    end;
    if pcols>1,
        set(gcf,'posi',[80 80 750 1000])
    end;
    emarkpl( filenam)
end;

figure;
plot(diff(t(:,:)))
xlabel ('sample #')
ylabel('diff(time) [sec]')
title('Sample time interval variation')
dtser = diff(t);
[Val, Bin]=hist(dtser(:),100);      % Val, Bin
line(Val*len/max(Val),Bin,'linew',3,'colo','k')
axis tight
tmu = mean(dtser(:));
tsi = std (dtser(:));
legend(['\mu= ' num2str(tmu*1e3) ', \sigma= ' num2str(tsi*1e3) ' [ms]'])
emarkpl( filenam);

means = zeros(3,n);
sigms = means;
s1=24;              % 1st sample
if findstr(filenam,'7775028')
    s1 = 10;
end;
xyzCln = cell(3,1); % each cell is X, Y or Z, no transient
linecol='bgrcmyk';          % colors order
for d = 1:3        % dim
    xyzCln{d} = squeeze(xyz(s1:len,d,:));      %176xn samples, dimension d, all n freqs
    mn = min(xyzCln{d}(:)); mx = max(xyzCln{d}(:));
    aa = [0 1 mn mx];   % for all subplot axes
    figure
    for i=1:n
        subplot(prows, pcols,i)
        plot(t(s1:len,i),xyzCln{d}(:,i),'.-')
        axis(aa); grid on
        title([ xyzstr(d) '-meas. for freq = ' num2str(f_in(i)) ' Hz'])
        %sqd = squeeze(txyz(s1:len,d+1,i));
        means(d,i) = mean(xyzCln{d}(:,i));
        sigms(d,i) = std (xyzCln{d}(:,i));
        xlabel(['[\mu \sigma]=[' num2str([means(d,i) sigms(d,i)]) ']'])
    end
    if pcols>1
        set(gcf,'posi',[80 80 750 1000])
    end;
    emarkpl( filenam)
end;

subplot(211)
%semilogx(f_in, means','.-')
plot(f_in, means','.-')
title('mean')
legend('X', 'Y', 'Z',0)
axis tight, grid on
subplot(212)
%semilogx(f_in, sigms','.-')
plot(f_in, sigms','.-')
title('sigma')
legend('X', 'Y', 'Z',0)
xlabel ('disturb. freq')
axis tight, grid on
emarkpl( filenam)

figure
for d=1:3
    subplot(3,1,d)
    stem3(f_in,t(s1:end,1), xyzCln{d},'.')
    view(-26,34)
    mesh(audiof,t(s1:end,1), squeeze(xyz(s1:end,d,:)))
    contour3(audiof,t(s1:end,1), squeeze(xyz(s1:end,d,:)))
    xlabel('freq[Hz]'), ylabel('time[sec]'), title(xyzstr(d))
    axis tight
end;

if pcols>1,
    set(gcf,'posi',[80 80 750 1000])
end
emarkpl( filenam);

Hs=spectrum.welch;
% EstimationMethod: 'Welch'
%    SegmentLength: 64
% OverlapPercent: 50
%     WindowName: 'Hamming'
% SamplingFlag: 'symmetric'
% psd(Hs,,'Fs',Fs)

axnum = zeros(3,1);
for ax = 1:3,        % dim x, y or z
    axnum(ax) = figure    ;
    for i=1:n
        subplot(prows, pcols,i)
        psd(Hs,xyzCln{d}(:,i),'Fs',Fs);
        %  set(get(gca,'chi'),'colo','m'),
        axis tight,
        aa=axis;
        ine (rem(f_in(i),Fs/2)*[1;1],aa(3:4),'color', 'r')
        he=legend('PSD',['Noise freq=' num2str(f_in(i)) ' Hz'],0);
        set(he,'fontsi',8, 'box', 'off');
    end;
    xlabel(['Frequency (Hz) Axis: ' xyzstr(ax)]);
end;
for ax = 1:3,
    figure(axnum(ax))
    if pcols>1,
        set(gcf,'posi',[80 80 750 1000])
    end
    emarkpl( filenam)
end;

Hs.SegmentLength = len-s1+1; % 176

axnum = zeros(3,1);
for ax = 1:3        % dim x, y or z
    % t_frqin = squeeze(txyz(s1:len,ax+1,:)); dimentions x freq.in
    axnum(ax) = figure    ;
    for i=1:n
        subplot(prows, pcols,i)
        psd(Hs,xyzCln{d}(:,i),'Fs',Fs);
        axis tight, aa=axis;
        line (rem(f_in(i),Fs/2)*[1;1],aa(3:4),'color', 'r')
        he=legend('PSD',['Noise freq=' num2str(f_in(i)) ' Hz']);
        set(he,'fontsi',8, 'box', 'off');
    end;
    xlabel(['Frequency (Hz) Axis: ' xyzstr(ax)]);
end;
for ax = 1:3
    figure(axnum(ax))
    if pcols>1,
        set(gcf,'posi',[80 80 750 1000])
    end
    emarkpl( filenam)
end;


function kern = rbfperiodic2KernExpandParam(kern, params)

% RBFPERIODIC2KERNEXPANDPARAM Create kernel structure from RBFPERIODIC2 kernel's parameters.
% FORMAT
% DESC returns a RBF periodic covariance with variying period kernel structure filled with the
% parameters in the given vector. This is used as a helper function to
% enable parameters to be optimised in, for example, the NETLAB
% optimisation functions.
% ARG kern : the kernel structure in which the parameters are to be
% placed.
% ARG param : vector of parameters which are to be placed in the
% kernel structure.
% RETURN kern : kernel structure with the given parameters in the
% relevant locations.
%
% SEEALSO : rbfperiodic2KernParamInit, rbfperiodic2KernExtractParam, kernExpandParam
%
% COPYRIGHT : Neil D. Lawrence, 2007, 2009
%
% MODIFICATIONS : Andreas C. Damianou, 2011
%
% MODIFICATIONS : Michalis K. Titsias, 2011

% KERN

kern.inverseWidth = params(1);
kern.variance = params(2);
kern.factor = params(3);
kern.period = 2*pi/kern.factor;