function y = mpower(x,d)
%MPOWER (overloaded)

if (numel(d)>1) || (size(x,1) ~= size(x,2))
   error('Inputs must be a scalar and a square matrix. To compute elementwise POWER, use POWER (.^) instead.');
end

if isa(d,'sdpvar')
    if numel(x) > 1
        if isa(x,'sdpvar')
            error('x^d not support SDPVARMATRIX^SDPVARSCALAR')
        else
            if isnumeric(x)
                [V,D] = eig(x);
                if ~isreal(D)
                    error('Matrix power x^d requires x to have real eigenvalues');
                end
                D = real(D);
                y = V*diag(diag(D).^d)*inv(V);
                y.extra.createTime = definecreationtime;
                return
            else
                error('Object class not support in x^d');
            end
        end
    end
    d = flush(d);d.conicinfo = [0 0];
    y = power_internal1(d,x);
    if isa(y,'sdpvar')
        y.extra.createTime = definecreationtime;
        y.extra.opname='';
    end
    return
end

x = flush(x);

% Trivial cases
if d==0
    y = eye(x.dim(1),x.dim(2))^0;
    return
end
if d==1
    y = x;
    return
end

% Check for special case norm(x)^2 which many users try to do
if d==2
    if length(x)==1
        base = getbase(x);
        if isequal(base,[0 1])
            if strcmp(x.extra.opname,'norm')
                model = yalmip('extstruct',getvariables(x));
                z = model.arg{1};
                if (isequal(model.arg{2},2) & min(size(z))==1) | isequal(model.arg{2},'fro')
                    z = reshape(model.arg{1},[],1);
                    y = real(z'*z);
                    y.extra.createTime = definecreationtime;
                     y.extra.opname='';
                    return
                end
            end
        end
    end
end

% Fractional and negative powers
if (ceil(d)-d>0) | (d<0)
    if x.dim(1)>1 | x.dim(2)>1
        error('Only scalars can have negative or non-integer powers');
    else
        base = getbase(x);
        if isequal(base,sparse([0 1])) % Simple unit scalar
            [mt,variabletype,hashM,hash] = yalmip('monomtable');
            var = getvariables(x);
          %  hash = randn(size(mt,2),1);
          %  hashM = mt*hash;
            hashV = (mt(var,:)*d)*hash;
            previous_var = find(abs(hashM - hashV) < 1e-20);
            if isempty(previous_var)
                newmt =  mt(getvariables(x),:)*d;
                mt(end+1,:) = newmt;
                yalmip('setmonomtable',mt,[variabletype newvariabletypegen(newmt)]);
                y = recover(size(mt,1));
            else
                y = recover(previous_var);
            end
        elseif  (size(base,2) == 2) & base(1)==0
            % Something like a*t^-d
            y = base(2)^d*recover(getvariables(x))^d;
        else
            % Bummer, something more complex, add an internal equality constraint
            y = (yalmip('define','mpower_internal',x))^d;
        end
    end
    y.extra.createTime = definecreationtime;
    y.extra.opname='';
    return
end

% Integer power of matrix
if x.dim(1)>1 | x.dim(2)>1
    switch d
        case 0
            y = 1;
        case 1
            y = x;
        otherwise
            y = x*mpower(x,d-1);
    end
else %Integer power of scalar

    base = x.basis;
    if isequal(base,[0 1]) % Unit scalar can be done fast
        [mt,variabletype,hashes,hash] = yalmip('monomtable');
        var = x.lmi_variables;
        newmt = mt(var,:)*d;
        newhash = newmt*hash;
        previous_var = findhash(hashes , newhash,length(hashes));
        if isempty(previous_var)
            mt = [mt;newmt];
            yalmip('setmonomtable',mt,[variabletype newvariabletypegen(newmt)]);
            y = x;
            y.lmi_variables = size(mt,1);
        else
            y = x;
            y.lmi_variables = previous_var;
        end

    else % General scalar
        switch d
            case 0
                y = 1;
            case 1
                y = x;
            case 2
                y = x*x;
            otherwise
                if even(d)
                    z = mpower(x,d/2);
                    y = z*z;
                else
                    y = x*mpower(x,d-1);
                end
        end
    end
    y.extra.createTime = definecreationtime;
    y.extra.opname='';
end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% function Qw = matVecQw(w,Q,flag)
%
% computes y = Q*w or Q'*w
% where Q =kron(Q3,kron(Q2,Q1)) is efficiently stored as Q= {Q1,Q2,Q3}
% see also splineTransformation2Dsparse
%==============================================================================

function Qw = matVecQw(w,Q,flag)

if nargin == 0,
  help(mfilename);
  return;
end;

% default is Q*w
if ~exist('flag','var'), flag = 'Qw';  end;

if strcmp(flag,'QTw'),
  % build transposes, note: small matrices
  for i=1:length(Q),    Q{i}=Q{i}';  end;
  Qw = matVecQw(w,Q);
  return;
end;

% reconstruct sizes m and p from Q and w, Q{j} is m{j}-by-p{j}

dim = length(Q);
for j=1:length(Q),
  p(j) = size(Q{j},2);
  m(j) = size(Q{j},1);
end;

switch dim,
  case 1,
    Qw = Q{1}*w;
  case 2,
    w  = reshape(w,[p,dim]);
    Qw = zeros(m(1),m(2),dim);
    for i =1:dim,
      Qw(:,:,i) = Q{1}*w(:,:,i)*Q{2}';
    end;
  case 3,
    w  = reshape(w,[p,dim]);
    Qw = zeros(m(1),m(2),m(3),dim);
    for i =1:dim,
        Qw(:,:,:,i) = tensorProd(Q,w(:,:,:,i));
    end;
end;

Qw = reshape(Qw,[],1);
%------------------------------------------------------------------------------

function Qw = tensorProd(Q,w);
for i=1:length(Q),
  p(i,:) = size(Q{i});
end;
Qw = zeros(p(:,1)');

for i1=1:size(Qw,1),
  for i2=1:size(Qw,2),
    for i3=1:size(Qw,3),
      for j1=1:size(w,1),
        for j2=1:size(w,2),
          for j3=1:size(w,3),
            Qw(i1,i2,i3) = Q{1}(i1,j1)*Q{2}(i2,j2)*Q{3}(i3,j3)*w(j1,j2,j3);
          end;
        end;
      end;
    end;
  end;
end;
%==============================================================================


% eml_osem_test.m
% compare aspire and matlab versions of E-ML-OSEM
% Copyright 2001-07-23, Jeff Fessler, The University of Michigan

% generate data
if ~isvar('xfbp'), printm 'data'
	if has_aspire
		f.dir	= test_dir;
		f.wtf	= [f.dir 't,g.wtf'];
		f.wtr	= strrep(f.wtf, 'wtf', 'wtr');
		f.yi	= [f.dir 'yi.fld'];
		f.ci	= [f.dir 'ci.fld'];
		f.ri	= [f.dir 'ri.fld'];
		f.mask	= [f.dir 'mask.fld'];
	end
	em_test_setup; f.is3b = 0;
%	em3_test_setup; f.is3b = 1;
prompt
end

% block object for block iterative
if ~isvar('Gb'), printm 'Gb'
	f.nblock = 5;
	Gb = Gblock(G, f.nblock);
end

% matlab iterations
if ~isvar('xmat'), printm 'matlab E-ML-OSEM'
	f.niter = 8;
	f.pixmax = 6;
	f.precon = 'fast';
	f.precon = 'classic';

	if f.is3b % fix: needs work!
		shaper = @(proj) reshape(proj, n.n1*n.n2, n.a);
	else
		shaper = @(x) x;
	end
	xinit = ig.ones;
	[xmat precon] = eml_osem(xinit(ig.mask), Gb, ...
			shaper(yi), shaper(ci), shaper(ri), ...
			'niter', f.niter, 'pixmax', f.pixmax, ...
			'precon', f.precon);
	precon = ig.embed(precon);
	xmat = ig.embed(xmat);
	im plc 1 2
	im(1, xmat, 'Matlab E-ML-OSEM iterations'), cbar horiz
prompt
end

if ~has_aspire, return, end

% aspire iterations
if ~isvar('xasp'), printm 'aspire E-ML-OSEM'

	f.init	= [f.dir 'init.fld'];
	f.out	= [f.dir 'out.fld'];
	fld_write(f.init, xinit, 'check', 0)
	if exist(f.out, 'file'), delete(f.out), end

	if ~isvar('f.sys_type')
		f.sys_type = ['2z@' f.wtr '@-'];
	end
	f.saver	= 'stack,1';
	f.alg	= sprintf('osemc,%s,%d,%d,1', f.precon, f.nblock, sg.na);
	f.penal	= '-';
	f.method = sprintf('@%d@%s@%s', f.niter, f.alg, f.penal);
	f.com = sprintf(['i -chat 5 empl3 %s %s  %s %s 1 %s 1 %s %s' ...
			' %s %s 0 1 %g 0 -'], ...
		f.out, f.init, f.yi, f.ci, f.ri, f.sys_type, f.mask, ...
		f.method, f.saver, f.pixmax);
	os_run(f.com)

	xasp = double(fld_read(f.out));
	im(2, xasp, 'Aspire E-ML-OSEM iterations'), cbar horiz
end

if 0 % test aspire preconditioner
	pasp = squeeze(fld_read('p.fld'));
	im plc 1 2
	im(1, precon, 'precon'), cbar
	im(2, pasp, 'aspire'), cbar
	max_percent_diff(precon, pasp)
	minmax(precon-pasp)
return
end

if 1
	t = vcorrcoef(xasp, xmat);
	printf('corr. %g,%g', t, t-1)

	im plc 2 2
	im(1, xmat, 'xhat matlab'), cbar
	im(2, xasp, 'xhat aspire'), cbar
	im(3, (xasp-xmat)/max(xmat(:)), 'aspire-matlab'), cbar

	t1 = eql_obj(xmat, G, yi(:), ci(:), ri(:), [], ig.mask);
	t2 = eql_obj(xasp, G, yi(:), ci(:), ri(:), [], ig.mask);

	if im
		subplot(224)
		plot(0:f.niter, t1-t1(1), '-o', 0:f.niter, t2-t1(1), '-x')
		xlabel iteration, ylabel '\Phi change', legend('mat', 'asp', 4)
		title(sprintf('E-ML-OSEM, Nsubset=%d', f.nblock))
	end
end


%% Housekeeping
% In this version, one can choose to run model 1 or model 2 by selecting
% the appropriate flag "ncp_shocks". If ncp_shocks==false, we have model 1
% otherwise we have model 2.
% Model 1: nonstationary and cointegrated
% - Neutral technology shocks
% - Investment-specific technology shocks
% Model 2: nonstationary and cointegrated
% - Neutral technology shocks
% - Investment-specific technology shocks
% - Preference shocks
% cointegrated
clc

%% RISE the model
ncp_shocks=true;
m=rise('sgusea12',...
    'rise_flags',struct('CountryNames',{{'H','F'}},'ncp_shocks',ncp_shocks),...
    'saveas',true);

%% Assign steady state file

m=set(m,'steady_state_file','sstate_model12b');

%% create parameters

[p,priors]=create_parameters12(ncp_shocks);

m=set(m,'parameters',p);

%% Bring in the data

data=create_data();

%% Estibrate the model
clc
ms=estimate(m,'data',data,'estim_priors',priors);
%%
[mfilt,loglik]=filter(m,'data',data);


function [ cost,gnd_objects,hyp_objects ] = roomLossFunction3D( gnd_objects, hyp_objects, H2G_R )
%ROOMLOSSFUNCTION Summary of this function goes here
%   Detailed explanation goes here

%% define cost
gnd_loss_cost = 2;
hyp_false_cost = 1;

%% convert hypothesis point
for i = 1:length(hyp_objects)
    hyp_objects(i).align = rotatePoint(hyp_objects(i).align, H2G_R);
end

%% greedy matching
test_type = [1:12 29];
test_type_num = length(test_type);
cost = 0;
hyp_objects(1).matchID = [];
hyp_objects(1).cost = [];

gnd_objects(1).matchID = [];
gnd_objects(1).cost = [];

for i = 1:test_type_num
    tid = test_type(i);

    hyp_rep_valid = [hyp_objects.objtype]==tid;
    hyp_rep_num = sum(hyp_rep_valid);
    hyp_rep = hyp_objects(hyp_rep_valid);
    hyp_rep_id = reshape(find(hyp_rep_valid),1,[]);

    gnd_rep_valid = [gnd_objects.objtype]==tid;
    gnd_rep_num = sum(gnd_rep_valid);
    gnd_rep = gnd_objects(gnd_rep_valid);
    gnd_rep_id = reshape(find(gnd_rep_valid),1,[]);

    if hyp_rep_num==0
        cost = cost + gnd_rep_num * gnd_loss_cost;
        for k = gnd_rep_id
            gnd_objects(k).cost = gnd_loss_cost;
            gnd_objects(k).matchID = 0;
        end

    elseif gnd_rep_num==0
        cost = cost + hyp_rep_num * hyp_false_cost;
        for k = hyp_rep_id
            hyp_objects(k).cost = hyp_false_cost;
            hyp_objects(k).matchID = 0;
        end

    else
        hyp_gnd_dist = zeros(hyp_rep_num, gnd_rep_num);
        hyp_gnd_maxs = zeros(hyp_rep_num, gnd_rep_num);
        for m = 1:hyp_rep_num
            for n = 1:gnd_rep_num

                hyp_gnd_dist(m,n) = verticesDistance(hyp_rep(m).align, gnd_rep(n).align);
                hyp_gnd_maxs(m,n) = 1;
%                 d = volumeIntersection(hyp_rep(m).point3D, gnd_rep(n).point3D);
%                 if d>=0
%                     hyp_gnd_dist(m,n) = 2*d;
%                     hyp_gnd_maxs(m,n) = 2;
%                 else
%                     hyp_gnd_dist(m,n) = verticesDistance(hyp_rep(m).point3D, gnd_rep(n).point3D);
%                     hyp_gnd_maxs(m,n) = 1;
%                 end

%                 [v1, v2, vi] = obbInteVolume(hyp_rep(m).point3D, gnd_rep(n).point3D);
%                 hyp_gnd_dist(m,n) = vi/(v1+v2-vi);
%                 p1 = hyp_rep(m).point3D;
%                 p2 = gnd_rep(n).point3D;
%                 meansize1 = (norm(p1(1,:)-p1(2,:)) + norm(p1(1,:)-p1(3,:)) + norm(p1(1,:)-p1(5,:)))/3;
%                 meansize2 = (norm(p2(1,:)-p2(2,:)) + norm(p2(1,:)-p2(3,:)) + norm(p2(1,:)-p2(5,:)))/3;
%                 meandist = mean(sqrt(sum((hyp_rep(m).point3D-gnd_rep(n).point3D).^2,2))) ...
%                     /sqrt(meansize1*meansize2);
%                 hyp_gnd_dist(m,n) = sigmoidFunc(meandist, 0.45, 4); %exp(-2*meandist);%

%                 if hyp_gnd_dist(m,n)>0.5
%                     fprintf('type: %d, hypid: %d, gndid: %d, score: %f\n', tid, hyp_rep_id(m), gnd_rep_id(n), hyp_gnd_dist(m,n));
%                 end

            end
        end

        sel_hyp = false(hyp_rep_num,1);
        sel_gnd = false(gnd_rep_num,1);
        for k = 1:min( hyp_rep_num, gnd_rep_num)
            [r,c] = find(hyp_gnd_dist==max(hyp_gnd_dist(:)));
            r = r(1); c = c(1);
            cost = cost + (hyp_gnd_maxs(r,c)-hyp_gnd_dist(r,c));
            hyp_objects(hyp_rep_id(r)).cost = (hyp_gnd_maxs(r,c)-hyp_gnd_dist(r,c))/2;
            hyp_objects(hyp_rep_id(r)).matchID = gnd_rep_id(c);
            gnd_objects(gnd_rep_id(c)).cost = (hyp_gnd_maxs(r,c)-hyp_gnd_dist(r,c))/2;
            gnd_objects(gnd_rep_id(c)).matchID = hyp_rep_id(r);

            sel_hyp(r) = true;
            sel_gnd(c) = true;
            hyp_gnd_dist(:,c) = -1;
            hyp_gnd_dist(r,:) = -1;
        end

        if hyp_rep_num>gnd_rep_num
            cost = cost + (hyp_rep_num-gnd_rep_num)*hyp_false_cost;
        else
            cost = cost + (gnd_rep_num-hyp_rep_num)*gnd_loss_cost;
        end
        rr = find(~sel_hyp);
        rc = find(~sel_gnd);
        for k = 1:length(rr)
            hyp_objects(hyp_rep_id(rr(k))).cost = hyp_false_cost;
            hyp_objects(hyp_rep_id(rr(k))).matchID = 0;
        end
        for k = 1:length(rc)
            gnd_objects(gnd_rep_id(rc(k))).cost = gnd_loss_cost;
            gnd_objects(gnd_rep_id(rc(k))).matchID = 0;
        end
    end

end

cost = cost/length(gnd_objects);
end

function d = verticesDistance(p1, p2)
% p1 = hyp_rep(m).point3D;
% p2 = gnd_rep(n).point3D;
meansize1 = (norm(p1(1,:)-p1(2,:)) + norm(p1(1,:)-p1(4,:)) + norm(p1(1,:)-p1(5,:)))/3;
meansize2 = (norm(p2(1,:)-p2(2,:)) + norm(p2(1,:)-p2(4,:)) + norm(p2(1,:)-p2(5,:)))/3;
meandist = mean(sqrt(sum((p1-p2).^2,2))) ...
    /sqrt(meansize1*meansize2);
d = sigmoidFunc(meandist, 0.45, 4); %exp(-2*meandist);%
end

% function d = volumeIntersection(p1, p2)
% [v1, v2, vi] = obbInteVolume(p1, p2);
% if v1==-1
%     d = -1;
% else
%     d = min(v1,v2)/vi;
% end
% end

function p = sigmoidFunc(v, m, a)
p = 1./(1+exp(a/m*(v-m)));
end

% gnd_objects_id = zeros(length(gnd_objects),1);
% gnd_valid = true(length(gnd_objects),1);
% gnd_xyz = zeros(length(gnd_objects),6);
% gnd_point = cell(length(gnd_objects),1);
% for i = 1:length(gnd_objects)
%     gnd_objects_id(i) = get_object_type({gnd_objects(i).name});
%     x = gnd_objects(i).x_w;
%     ad = abs(round(x(7)/(pi/2))*(pi/2) - x(7));
%     if ad>0.1
%         gnd_valid(i) = false;
%     end
%     out_points_w = gnd_objects(i).out_points_w;
%     gnd_xyz(i,:) = [min(out_points_w, [], 1) max(out_points_w, [], 1)];
%     gnd_point{i} = anno2point(out_points_w, annorule(rule_type==gnd_objects(i).type));
% end
%
%
% hyp_objects_id = zeros(length(hyp_objects),1);
% hyp_xyz = zeros(length(hyp_objects),6);
% hyp_point = zeros(8,3,length(hyp_objects));
% for i = 1:length(hyp_objects)
%     hyp_objects_id(i) = get_object_type({hyp_objects(i).name});
%     out_points_w = hyp_objects(i).out_points_w;
%     hyp_xyz(i,:) = [min(out_points_w, [], 1) max(out_points_w, [], 1)];
% end
% hyp_valid = true(length(hyp_objects),1);
%
% subset_category = [1:12 29];
% XYZ2POINTLIST = [1 2 3; 4 2 3; 1 5 3; 4 5 3; ...
%                  1 2 6; 4 2 6; 1 5 6; 4 5 6];
%
%
% %% check precision
% categorycost = inf*ones(length(subset_category),1);
% categorynumb = zeros(length(subset_category),2);
% hyp_hit_id = zeros(length(hyp_objects),2);
% for k = 1:length(subset_category)
%     oid = subset_category(k);
%     gnd_ids = find(gnd_valid & gnd_objects_id==oid);
%     hyp_ids = find(hyp_objects_id==oid);
%     num_gnd = length(gnd_ids);
%     num_hyp = length(hyp_ids);
%     categorynumb(k,:) = [num_hyp num_gnd];
%     if num_hyp == 0 && num_gnd~=0;
%         categorycost(k) = 860530;
%         continue;
%     elseif num_gnd == 0 && num_hyp~=0
%         categorycost(k) = 860530;
%         continue;
%     elseif num_gnd == 0 && num_hyp == 0
%         categorycost(k) = 0;
%         continue;
%     end
%
%     distance = zeros(length(hyp_ids), length(gnd_ids));
%     gnd_pl = zeros(length(gnd_ids)*8,3);
%     for i = 1:length(gnd_ids)
%         xyz = gnd_xyz(gnd_ids(i),:);
%         gnd_pl(i*8-7:i*8,:) = xyz(XYZ2POINTLIST);
%     end
%     for i = 1:length(hyp_ids)
%         xyz = hyp_xyz(hyp_ids(i),:);
%         hyp_pl = xyz(XYZ2POINTLIST);
%         dist = sqrt(sum((repmat(hyp_pl, length(gnd_ids), 1) - gnd_pl).^2,2));
%         dist = sum(reshape(dist, 8, []),1);
%         distance(i,:) = dist;
%     end
%
%     assigned_id = zeros(num_hyp, 1);
%     assigned_gnd = false(num_gnd,1);
%     assigned = false(num_hyp, 1);
%     assigned_dist = inf*ones(num_hyp, 1);
%     while ~all(assigned) && ~all(assigned_gnd)
%         [B,I] = min(distance, [], 2);
%         [C,J] = min(B);
%         assigned(J) = true;
%         assigned_gnd(I(J)) = true;
%         assigned_id(J) = I(J);
%         assigned_dist(J) = C;
%         hyp_hit_id(hyp_ids(J),:) = [gnd_ids(I(J)) C];
%
%         distance(J,:) = inf;
%         distance(:,I(J)) = inf;
%     end
%
%     max_penalty = 2*max(assigned_dist(~isinf(assigned_dist)));
%     assigned_dist(isinf(assigned_dist)) = max_penalty;
%     miss_gnd_num = max(0, num_gnd-num_hyp);
% %     try
%     categorycost(k) = mean([assigned_dist; max_penalty*ones(miss_gnd_num, 1)]);
% %     catch
% %         fprintf('');
% %     end
% end
%
% cost = mean(categorycost(~isinf(categorycost)));
%
% end
%
% function points = anno2point(out_points, annorule)
% inv_vertex = annorule.inv_vertex;
% for i = 1:size(inv_vertex,1)
%     out_points(end+1,:) = out_points(inv_vertex(i,1),:) ...
%                         + out_points(inv_vertex(i,2),:) ...
%                         - out_points(inv_vertex(i,3),:);
% end
% points = out_points;
% end


function [solution] = enumerateOptimalSolutions(model)
% Returns a set of optimal flux distributions
% spanning the optimal set
%
% USAGE:
%
%    [solution] = enumerateOptimalSolution(model)
%
% INPUT:
%    model:       COBRA model structure
%
% OUTPUT:
%    solution:    solution structure
%
%                   * fluxes - Flux distribution for each iteration
%                   * nonzero - Boolean matrix denoting which fluxes are nonzero for each iteration
%
% .. Authors:
%       - Jan Schellenberger, August 2008 - Based on code by Jennie Reed
%       - Reed, J.L. and Palsson, B.O., "Genome-scale in silico models of ''E. coli'' have multiple equivalent phenotypic states: assessment of correlated reaction subsets that comprise network states" , Genome Research, 14:1797-1805(2004).

[nMets,nRxns] = size(model.S);

solution.fluxes = zeros(nRxns,0);
solution.nonzero = zeros(nRxns,0);

%sol = optimizeCbModel(model);
LPproblem = buildLPproblemFromModel(model);
NZ = zeros(nRxns,0);
sol = solveCobraLP(LPproblem);
tol = getCobraSolverParams('LP','feasTol');
NZ(:,end+1) = abs(sol.full(1:nRxns))>tol;
PrevNZ = NZ(:,end);
PrevFW = sol.full(1:nRxns) > tol;
solution.fluxes(:,end+1) = sol.full(1:nRxns);

maxObjective = sol.obj;
MILPproblem = LPproblem;
% Add a Line to keep the objective value
MILPproblem.A = [MILPproblem.A;LPproblem.c'];
MILPproblem.b = [MILPproblem.b;maxObjective];
if MILPproblem.osense == 1
    sense = 'L';
else
    sense = 'G';
end
MILPproblem.csense = [MILPproblem.csense;sense];

% add the indicator variables to the LP:

[nCtrs,nVars] = size(MILPproblem.A);
% v_+ / v_-
MILPproblem.A = [MILPproblem.A,sparse(nCtrs,nRxns*2);...
               speye(nRxns,nVars), -diag(model.ub), sparse(nRxns,nRxns);... %v - v_+*ub <= 0
               speye(nRxns,nVars+nRxns), -diag(model.lb);....%v - v_-*lb >= 0
               sparse(nRxns,nVars),speye(nRxns,nRxns), speye(nRxns,nRxns)];%;...% v_i+ + v_i- <= 1
               %sparse(1,nVars),~(PrevFW & PrevNZ)',~(~PrevFW & PrevNZ)']; %At least one reaction that was not yet active has to be active now!
MILPproblem.vartype = [repmat('C',nVars,1);repmat('B',2*nRxns,1)];
MILPproblem.lb = [MILPproblem.lb;zeros(2*nRxns,1)];
MILPproblem.ub = [MILPproblem.ub;ones(2*nRxns,1)];
MILPproblem.csense = [MILPproblem.csense;...
                    repmat('L',nRxns,1);...
                    repmat('G',nRxns,1);...
                    repmat('L',nRxns,1)];
MILPproblem.b = [MILPproblem.b;...
               zeros(2*nRxns,1);...
               ones(nRxns,1)];

MILPproblem.c = [LPproblem.c; zeros(2*nRxns,1)];

NonZeroConstPos = size(MILPproblem.A,1);
while 1
    % modify the "At least one new reaction" constraint
    %MILPproblem.A(NonZeroConstPos,:) = [sparse(1,nVars),~PrevNZ',~PrevNZ'];
    % add the "this solution is no longer allowed constraint
    MILPproblem.A = [MILPproblem.A;...
                   sparse(1,nVars), (PrevFW & PrevNZ)', (~PrevFW & PrevNZ)'];
    MILPproblem.csense = [MILPproblem.csense; 'L'];
    MILPproblem.b = [MILPproblem.b; sum(PrevNZ)-1];
    solMILP = solveCobraMILP(MILPproblem);
    if solMILP.stat ~= 1
        % No more solutions can be found
        solution.nonzero = NZ;
        return;
    end
    NZ(:,end+1) = abs(solMILP.full(1:nRxns))>tol;
    PrevNZ = NZ(:,end);
    PrevFW = solMILP.full(1:nRxns) > tol;
    solution.fluxes(:,end+1) = solMILP.full(1:nRxns);
end


classdef bdepth_ratio < ZmapHGridFunction
% BDEPTH_RATIO compare b values at two different depths
% Stefan Wiemer 1/95

    properties
        topzone_ceiling         = 0;    % Top of TOP zone
        topzone_floor           = 5;    % Bottom of TOP zone
        bottomzone_ceiling      = 7;    % Top of BOTTOM zone
        bottomzone_floor        = 15;   % Bottom of BOTTOM zone
        fBinning                = 0.1;  % magnitude bins
        mc_choice   McMethods   = McMethods.MaxCurvature % magnitude of completion method
        useAutoMcomp McAutoEstimate = true
    end

    properties(Constant)
        PlotTag='bdepth'
        ReturnDetails = cell2table({ ... VariableNames, VariableDescriptions, VariableUnits
            ...
            ... % these are returned from the calculation
            'bv_ratio',     'b-value Ratio Map',                            '';... %valueMap [old] #1
            'magco',        'Magnitude of completion (bottom)',             'mag';... #2
            'bv2',          'b-value Ratio map (with autoestimate Mcomp)',  '';... #3
            'av',           'a-value ratios',                   '';... #4 avm
            'Prmap',        'Utsu Probability map',             '';... #5 Prmap
            'top_b',        'Top Zone b-value map',             '';... #6 tob_b
            'bottom_b',     'Bottom Zone b-value map',          '';... #7 bottom_b
            'per_top',      '% of nodal EQs within TOP zone',   '';... #8 per_in_top
            'per_bot',      '% of nodal EQs within BOTTOM zone','';... #9 per_in_bot
            'Number_of_Events_top', 'Number of events in TOP zone',     '';... #10
            'Number_of_Events_bot', 'Number of events in BOTTOM zone',  '';... #11
            ...
            }, 'VariableNames', {'Names','Descriptions','Units'})

        CalcFields = {...
            'bv_ratio',     'magco',    'bv2',      'av',       'Prmap',...
            'top_b',        'bottom_b', 'per_top',  'per_bot',...
            'Number_of_Events_top',     'Number_of_Events_bot'};

        ParameterableProperties = ["topzone_ceiling" "topzone_floor"....
                    "bottomzone_ceiling" "bottomzone_floor"...
                    "fBinning" "NodeMinEventCount" "mc_choice" "useAutoMcomp"];

        References="";
    end

    methods
        function obj = bdepth_ratio(zap, varargin)
            % BDEPTH_RATIO compare b values at two different depths

            obj@ZmapHGridFunction(zap, 'bv_ratio');
            report_this_filefun();

            obj.parseParameters(varargin);
            obj.StartProcess();

        end

        function InteractiveSetup(obj)

            % get two time periods, along with grid and event parameters
            zdlg=ZmapDialog();
            zdlg.AddMcAutoEstimateCheckbox('useAutoMcomp',  obj.useAutoMcomp);
            zdlg.AddMcMethodDropdown('mc_choice',           obj.mc_choice);
            zdlg.AddHeader('Please define two Depth ranges to compare');
            zdlg.AddEdit('topzone_ceiling','TOP zone ceiling [km]',         obj.topzone_ceiling,'');
            zdlg.AddEdit('topzone_floor','TOP zone floor [km]',        obj.topzone_floor,'');
            zdlg.AddEdit('bottomzone_ceiling','BOTTOM zone ceiling [km]',   obj.bottomzone_ceiling,'');
            zdlg.AddEdit('bottomzone_floor','BOTTOM zone floor [km]',  obj.bottomzone_floor,'');
            obj.AddDialogOption(zdlg, 'NodeMinEventCount');
            obj.AddDialogOption(zdlg,'EventSelector');
            zdlg.Create('Name', 'Depth Ratio Grid input parameters','WriteToObj',obj,'OkFcn',@obj.doIt);
        end

        function modifyGlobals(obj)
            obj.ZG.bvg = obj.Result.values;
        end

        function results=Calculate(obj)

            assert(obj.topzone_ceiling < obj.topzone_floor, 'TOP ZONE: zone floor is above zone ceiling');
            assert(obj.bottomzone_ceiling < obj.bottomzone_floor, 'BOTTOM ZONE: zone floor is above zone ceiling');

            %  make grid, calculate start- endtime etc.  ...
            %

            % find row index of ratio midpoint
            l =  obj.topzone_ceiling <= obj.RawCatalog.Depth & obj.RawCatalog.Depth <  obj.topzone_floor;
            top_zone = obj.RawCatalog.subset(l);

            l = obj.bottomzone_ceiling <= obj.RawCatalog.Depth & obj.RawCatalog.Depth <  obj.bottomzone_floor;
            bot_zone = obj.RawCatalog.subset(l);

            % overall b-value
            [top_b_overall] = bvalca3(top_zone.Magnitude, obj.useAutoMcomp);
            [bottom_b_overall] = bvalca3(bot_zone.Magnitude, obj.useAutoMcomp);

            depth_ratio = top_b_overall/bottom_b_overall;
            disp(depth_ratio);

            [~, mcCalculator] = calc_Mc([], obj.mc_choice,obj.fBinning);

            % loop over all points
            obj.gridCalculations(@calculation_function);

            obj.Result.top.depthrange_km=[obj.topzone_ceiling obj.topzone_floor];
            obj.Result.bottom.depthrange_km=[obj.bottomzone_ceiling obj.bottomzone_floor];

            if nargout
                results=obj.Result.values;
            end

            %catsave3('bdepth_ratio');

            % to View the b-value map : view_bdepth

            function out=calculation_function(b)
                topb = b.subset( obj.topzone_ceiling <= b.Depth & b.Depth <  obj.topzone_floor );
                per_in_top = (topb.Count/b.Count)*100.0;

                botb = b.subset( obj.bottomzone_ceiling<= b.Depth & b.Depth <  obj.bottomzone_floor );
                per_in_bot = (botb.Count/b.Count)*100.0;

                if length(topb) < obj.NodeMinEventCount  || length(botb) < obj.NodeMinEventCount

                    [Mc_valueTop] = mcCalculator(topb);
                    [Mc_valueBot] = mcCalculator(botb);
                    [topbv, topbv2, ~, topav, n1]=calc_bval_both_ways(topb,Mc_valueTop, top_b_overall);
                    [botbv, botbv2, magco, botav, n2]=calc_bval_both_ways(botb,Mc_valueBot, bottom_b_overall);
                else
                    [topbv, topbv2, ~, topav,  n1]=deal(nan);
                    [botbv, botbv2, magco, botav, n2]=deal(nan);
                end
                bv = topbv/botbv;
                bv2 = topbv2/botbv2;
                av = topav/botav;

                n = n1+n2;
                obj.ZG.overall_b_value = topbv;
                da = -2*n*log(n) + 2*n1*log(n1+n2 * topbv/botbv) + 2*n2*log(n1 * botbv/topbv + n2) - 2;
                pr = (1  -  exp(-da/2-2))*100;

                ltopb = topb.Count;
                lbotb = botb.Count;

                out = [bv magco ...
                    bv2 av pr ...
                    topbv botbv ...
                    per_in_top per_in_bot ...
                    ltopb lbotb];

                function [bv, bv2, magco, av, n] = calc_bval_both_ways(mycat,magco,b_overall)
                    % where mycat is already the subset value
                    idx = (magco-0.05) <= mycat.Magnitude5;
                    n=sum(idx);
                    if obj.NodeMinEventCount <= sum(idx)
                        [bv, magco, ~, av] =  bvalca3(mycat.Magnitude(idx), McAutoEstimate.manual, b_overall); %not automatic estimate of Mcomp
                        bv2 =  bvalca3(mycat.Magnitude(idx), McAutoEstimate.auto); % automatic estimate of Mcomp
                    else
                        [bv, bv2, magco, av] = deal(nan);
                    end
                end
            end
        end % Calculate

    end %methods

    methods(Static)
        function h = AddMenuItem(parent, zapFcn, varargin)
            % create a menu item
            label = 'b-value depth ratio grid';
            h = uimenu(parent, 'Label', label,...
                'MenuSelectedFcn', @(~,~)XYfun.bdepth_ratio(zapFcn()),...
                varargin{:});
        end

        function obj=my_load()
            unimplemented_error();
            % Load exist b-grid
            [file1,path1] = uigetfile(['*.mat'],'b-value gridfile');
            if length(path1) > 1

                load([path1 file1])
                normlap2=nan(length(tmpgri(:,1)),1)

                normlap2(ll)= bvg(:,1);
                valueMap=reshape(normlap2,length(yvect),length(xvect));

                normlap2(ll)= bvg(:,5);
                r=reshape(normlap2,length(yvect),length(xvect));

                normlap2(ll)= bvg(:,6);
                meg=reshape(normlap2,length(yvect),length(xvect));

                normlap2(ll)= bvg(:,2);
                old1=reshape(normlap2,length(yvect),length(xvect));

                %  normlap2(ll)= bvg(:,7);
                %  pro=reshape(normlap2,length(yvect),length(xvect));

                normlap2(ll)= bvg(:,7);
                avm=reshape(normlap2,length(yvect),length(xvect));

                %  normlap2(ll)= bvg(:,9);
                % stanm=reshape(normlap2,length(yvect),length(xvect));

                normlap2(ll)= bvg(:,8);
                Prmap=reshape(normlap2,length(yvect),length(xvect));

                normlap2(ll)= bvg(:,9);
                top_b=reshape(normlap2,length(yvect),length(xvect));

                normlap2(ll)= bvg(:,10);
                bottom_b=reshape(normlap2,length(yvect),length(xvect));

                normlap2(ll)= bvg(:,11);
                per_top=reshape(normlap2,length(yvect),length(xvect));

                normlap2(ll)= bvg(:,12);
                per_bot=reshape(normlap2,length(yvect),length(xvect));

                %    normlap2(ll)= bvg(:,13);
                %    ltopb=reshape(normlap2,length(yvect),length(xvect));

                %   normlap2(ll)= bvg(:,14);
                %  lbotb=reshape(normlap2,length(yvect),length(xvect));

                old = valueMap;

                view_bdepth
            else
                return
            end
        end
    end
end


function [x,ebuf,Zi] = celpsyn(cb,kappa,k,theta0,P,b,ebuf,Zi)
%  celpsyn --> CELP synthesizer (decoder).
%
%    [x,ebuf,Zi] = celpsyn(cb,kappa,k,theta0,P,b,ebuf,Zi)
%
%    The CELP synthesizer consists of the cascade of the pitch filter
%    and the vocal-tract filter with excitation signal taken from the
%    codebook.
%
%      ------------  Gain, theta0 ----------------      ---------
%      | Gaussian |          |    |      1       |      |   1   |
%      | codebook |--------->X--->| ------------ |----->| ----- |--->
%      |   cb     | rho_k(n)      | 1 - b*z^(-P) | e(n) |  A(z) | x(n)
%      ------------               ----------------      ---------
%
%    First, the Gaussian codebook given by the L-by-K matrix cb, and
%    the excitation parameters k, theta0, P, and b, are used to generate
%    the excitation sequence, e(n). This is done in blocks of length L,
%    so if the length of the parameter vector k is J, then L*J samples
%    of e(n) are generated. The vector ebuf contains previous excitation
%    samples due to the memory hangover in the pitch filter, and the
%    length of this vector must at least be the max possible pitch period.
%
%    Then, the excitation sequence, e(n), is filtered by the vocal-tract
%    filter, 1/A(z), where the coefficients are obtained from the
%    reflection coefficients, kappa. Zi is the memory hangover in this
%    filter.
%

[L,K] = size(cb);                       % Block length and codebook size.
F = length(ebuf);                       % No. of previous excitation samples.
J = length(k);                          % No. blocks per frame.
N = L*J;                                % Frame length.

e = zeros(N,1);

for (j=1:J)
  n = (j-1)*L+1:j*L;

 % Find the signal e(n) based on the parameters b, P, theta0, and k.
 if (P(j) < L)
    Zp   = b(j)*ebuf(F-P(j)+1:F);
    e(n) = filter(1,[1 zeros(1,P(j)-1) -b(j)],theta0(j)*cb(:,k(j)),Zp);
  else
    e(n) = theta0(j)*cb(:,k(j)) + b(j)*ebuf(F-P(j)+1:F-P(j)+L);
  end
  ebuf = [ebuf(L+1:F); e(n)];           % Update e(n) buffer.
end

a = rf2lpc(kappa);                      % Convert kappa to a parameters.
[x,Zi] = filter(1,[1; -a],e,Zi);        % Vocal-tract filter.


function varargout = interp_gridded(transform, val, pos_to, varargin)

% INTERP_GRIDDED computes a matrix that interpolates values that were
% observed on positions in a regular 3-D grid onto positions that are
% unstructured, e.g. the vertices of a cortical sheet.
%
% Use as
%   [val]                = interp_gridded(transform, val, pos, ...) or
%   [interpmat, distmat] = interp_gridded(transform, val, pos, ...)
% where
%   transform  homogenous coordinate transformation matrix for the volume
%   val        3-D matrix with the values in the volume
%   pos        Mx3 matrix with the vertex positions onto which the data should
%              be interpolated
%
% Optional arguments are specified in key-value pairs and can be
%    projmethod   = 'nearest', 'sphere_avg', 'sphere_weighteddistance'
%    sphereradius = number
%    distmat      = NxM matrix with precomputed distances
%    inside       = indices for inside voxels (or logical array)

% Copyright (C) 2007-2015, Jan-Mathijs Schoffelen & Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if nargin<3
  ft_error('Not enough input arguments.');
end

% get the optional arguments
projvec      = ft_getopt(varargin, 'projvec',       1);
projweight   = ft_getopt(varargin, 'projweight',    ones(size(projvec)));
projcomb     = ft_getopt(varargin, 'projcomb',      'mean'); % or max
projthresh   = ft_getopt(varargin, 'projthresh',    []);
projmethod   = ft_getopt(varargin, 'projmethod');    % required
sphereradius = ft_getopt(varargin, 'sphereradius');  % required for some projection methods
distmat      = ft_getopt(varargin, 'distmat');       % will be computed if not present
inside       = ft_getopt(varargin, 'inside');

dim    = size(val);
dimres = svd(transform(1:3,1:3)); % to reduce the number of elements in the distance matrix
npos_to   = size(pos_to,1);
npos_from = prod(dim);

if isempty(distmat)
  % compute the distance matrix
  switch projmethod
    case 'nearest'
      % determine the nearest voxel for each vertex
      sub = round(ft_warp_apply(inv(transform), pos_to, 'homogenous'));  % express
      sub(sub(:)<1) = 1;
      sub(sub(:,1)>dim(1),1) = dim(1);
      sub(sub(:,2)>dim(2),2) = dim(2);
      sub(sub(:,3)>dim(3),3) = dim(3);
      ind = sub2ind(dim, sub(:,1), sub(:,2), sub(:,3));
      distmat = sparse(1:npos_to, ind, ones(size(ind)), npos_to, npos_from);
      if ~isempty(inside)
        % only voxels inside the brain contain a meaningful functional value
        distmat = distmat(:, inside);
      end

    case {'sphere_avg', 'sphere_weighteddistance'}
      if isempty(sphereradius)
        ft_error('sphereradius should be specified');
      end

      [X, Y, Z] = ndgrid(1:dim(1), 1:dim(2), 1:dim(3));
      pos_from  = ft_warp_apply(sparse(transform), [X(:) Y(:) Z(:)]);
      % the distance only has to be computed to voxels inside the brain
      pos_from  = pos_from(inside,:);
      npos_from = size(pos_from,1);
      % compute the distance between all voxels and each surface point
      dfromsq = sum(pos_from.^2,2); % squared distance to origin
      dtosq   = sum(pos_to.^2,  2); % squared distance to origin
      maxnpnt = double(npos_to*ceil(4/3*pi*(sphereradius/max(dimres))^3)); % initial estimate of nonzero entries
      distmat = spalloc(npos_to, npos_from, maxnpnt);
      ft_progress('init', 'textbar', 'computing distance matrix');
      for j = 1:npos_to
        ft_progress(j/npos_to);
        d   = sqrt(dfromsq + dtosq(j) - 2 * pos_from * pos_to(j,:)');
        sel = find(d<sphereradius);
        distmat(j, sel) = single(d(sel)) + eps('single');
      end
      ft_progress('close');

    case 'project'
      % do nothing I believe

    otherwise
      ft_error('unsupported projection method');
  end % case projmethod
end % if isempty distmat

%------do something with the distance matrix
switch projmethod
  case 'nearest'
    projmat         = distmat;

  case 'sphere_avg'
    projmat         = distmat;
    [ind1, ind2, d] = find(projmat);
    nnz             = full(sum(spones(projmat),2));
    for k = 1:length(ind1)
      projmat(ind1(k),ind2(k)) = 1./nnz(ind1(k));
    end

  case 'sphere_weighteddistance'
    projmat         = distmat;
    [ind1, ind2, d] = find(projmat);
    projmat         = sparse(ind1, ind2, 1./d, npos_to, npos_from);
    [ind1, ind2, d] = find(projmat);
    normnz          = sqrt(full(sum(projmat.^2, 2)));
    projmat         = sparse(ind1, ind2, d./normnz(ind1), npos_to, npos_from);

  case 'project'
      % this method is Joachim's implementation that was originally in
      % ft_sourceplot, it assumes the functional data to be defined on a
      % regular 3D grid, and that the transformation to world-space is known

      % we also need the dim
      dim = ft_getopt(varargin, 'dim');
      dat = zeros(size(pos_to,1),1);

      % convert projvec in mm to a factor, assume mean distance of 70mm
      projvec = (70-projvec)/70;
      for iproj = 1:length(projvec),
        sub = round(ft_warp_apply(inv(transform), pos_to*projvec(iproj), 'homogenous'));  % express
        sub(sub(:)<1) = 1;
        sub(sub(:,1)>dim(1),1) = dim(1);
        sub(sub(:,2)>dim(2),2) = dim(2);
        sub(sub(:,3)>dim(3),3) = dim(3);
        ind = sub2ind(dim, sub(:,1), sub(:,2), sub(:,3));
        if strcmp(projcomb,'mean')
          dat = dat + projweight(iproj) * val(ind);
        elseif strcmp(projcomb,'max')
          dat  = max([dat projweight(iproj) * val(ind)],[],2);
          tmp2 = min([dat projweight(iproj) * val(ind)],[],2);
          fi   = find(dat < max(tmp2));
          val(fi) = tmp2(fi);
        else
          ft_error('undefined method to combine projections; use cfg.projcomb= mean or max')
        end
      end
      if strcmp(projcomb,'mean'),
        dat = dat/length(projvec);
      end
      %     if ~isempty(projthresh),
      %       mm=max(abs(val(:)));
      %       maskval(abs(val) < projthresh*mm) = 0;
      %     end
      %

  otherwise
    ft_error('unsupported projection method');
end  % case projmethod

if nargout==1 && ~strcmp(projmethod, 'project')
  % return the interpolated values
  varargout{1} = projmat * val(:);
elseif nargout==1
  varargout{1} = dat;
else
  % return the interpolation and the distance matrix
  varargout{1} = projmat;
  varargout{2} = distmat;
end


function variance = circular_normal_01_variance ( )

%*****************************************************************************80
%
%% CIRCULAR_NORMAL_01_VARIANCE returns the variance of the Circular Normal 01 PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VARIANCE(2), the variance of the PDF.
%
  variance(1:2) = 1.0;

  return
end


function z = triumtriu(x,y,K)
% z = triumtriu(x,y,K)
%
% TRIUMTRIU  Computes y = x * y
%   Both r and u should be upper triangular.
%
% **********  INTERNAL FUNCTION OF SEDUMI **********
%
% See also sedumi

% This file is part of SeDuMi 1.1 by Imre Polik and Oleksandr Romanko
% Copyright (C) 2005 McMaster University, Hamilton, CANADA  (since 1.1)
%
% Copyright (C) 2001 Jos F. Sturm (up to 1.05R5)
%   Dept. Econometrics & O.R., Tilburg University, the Netherlands.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% Affiliation SeDuMi 1.03 and 1.04Beta (2000):
%   Dept. Quantitative Economics, Maastricht University, the Netherlands.
%
% Affiliations up to SeDuMi 1.02 (AUG1998):
%   CRL, McMaster University, Canada.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc.,  51 Franklin Street, Fifth Floor, Boston, MA
% 02110-1301, USA

Ks = K.s;
if isempty(Ks)
    z = [];
    return
end
Kq = Ks .* Ks;
nr = K.rsdpN;
nc = length(Ks);
N  = K.N - K.sblkstart(1) + 1;
z  = zeros(N,1);
xi = length(x) - N;
zi = 0;
for i = 1 : nc
    ki = Ks(i);
    qi = Kq(i);
    XX = x(xi+1:xi+qi);
    YY = y(xi+1:xi+qi);
    xi = xi + qi;
    if i > nr
        XX = XX + 1j * x(xi+1:xi+qi);
        YY = YY + 1j * y(xi+1:xi+qi);
        xi = xi + qi;
    end
    ZZ = triu(reshape(XX,ki,ki)) * triu(reshape(YY,ki,ki));
    ZZ = ZZ + triu(ZZ,1)';
    z(zi+1:zi+qi) = real(ZZ);
    zi = zi + qi;
    if i > nr
        z(zi+1:zi+qi) = imag(ZZ);
        zi = zi + qi;
    end
end


function [IND_mask]=maskfind(M,IND,MASK_I,MASK_J,MASK_K)

% function [IND_mask]=maskfind(M,IND,I,J,K)
% ------------------------------------------------------------------------
% This function finds the indices of the elements found inside the mask
% defined by MASK_I, MASK_J and MASK_K. The matrix IND_mask is numel(IND) x
% numel(MASK_I) in size. N.B. zeros are used when indices are not found.
%
% Kevin Mattheus Moerman
% kevinmoerman@hotmail.com
% 07/04/2010
% ------------------------------------------------------------------------

%%

%Get image size
siz=size(M);
if size(M,3)==1
    siz(3)=1;
end

%Force row vector input
MASK_I=MASK_I(:)';
MASK_J=MASK_J(:)';
MASK_K=MASK_K(:)';

%Get subscript indices for IND
[Im,Jm,Km]=ind2sub(siz,IND(:));

%Create adjacency subscript indices
Im=Im(:,ones(1,numel(MASK_I)))+MASK_I(ones(numel(Im),1),:);
Jm=Jm(:,ones(1,numel(MASK_J)))+MASK_J(ones(numel(Jm),1),:);
Km=Km(:,ones(1,numel(MASK_K)))+MASK_K(ones(numel(Km),1),:);

%Get logic for valid indices.
L_valid= ~((Im<1 | Im>siz(1)) | (Jm<1 | Jm>siz(2)) | (Km<1 | Km>siz(3)));

%Get linear indiced of adjacenty indices
[ind_m]=sub2ind(siz,Im(L_valid),Jm(L_valid),Km(L_valid));

IND_mask=zeros(size(Im));
IND_mask(L_valid)=ind_m;

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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/>.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% chrelerr.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function flag = chrelerr(fbest,stop)
% checks whether the required tolerance for a test function with known
% global minimum has already been achieved
% Input:
% fbest		function value to be checked
% stop(1)	relative error with which a global minimum with not too
%		small absolute value should be reached
% stop(2)	global minimum function value of a test function
% stop(3)	if abs(fglob) is very small, we stop if the function
%		value is less than stop(3)
% Output:
% flag          = 0 the required tolerance has been achieved
% 		= 1 otherwise

function flag = chrelerr(fbest,stop)
fglob = stop(2);
if fbest - fglob <= max(stop(1)*abs(fglob),stop(3))
  flag = 0;
else
  flag = 1;
end


% RES = pyrBandIndices(INDICES, BAND_NUM)
%
% Return indices for accessing a subband from a pyramid
% (gaussian, laplacian, QMF/wavelet, steerable).

% Eero Simoncelli, 6/96.

function indices =  pyrBandIndices(pind,band)

if ((band > size(pind,1)) | (band < 1))
  error(sprintf('BAND_NUM must be between 1 and number of pyramid bands (%d).', ...
      size(pind,1)));
end

if (size(pind,2) ~= 2)
  error('INDICES must be an Nx2 matrix indicating the size of the pyramid subbands');
end

ind = 1;
for l=1:band-1
  ind = ind + prod(pind(l,:));
end

indices = ind:ind+prod(pind(band,:))-1;


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

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defun fresnels (@var{x})
%% Numerical fresnels function.
%%
%% Example:
%% @example
%% @group
%% @c doctest: +SKIP_IF(compare_versions (OCTAVE_VERSION(), '6.0.0', '<'))
%% fresnels (1.1)
%%   @result{} ans = 0.5365
%% @end group
%% @end example
%%
%% @strong{Note} this function may be slow for large numbers of inputs.
%% This is because it is not a native double-precision implementation
%% but rather the numerical evaluation of the Python @code{mpmath} function
%% @code{fresnels}.
%%
%% Note: this file is autogenerated: if you want to edit it, you might
%% want to make changes to 'generate_functions.py' instead.
%%
%% @seealso{@@sym/fresnels}
%% @end defun

function y = fresnels (x)
  if (nargin ~= 1)
    print_usage ();
  end
  cmd = { 'L = _ins[0]'
          'A = [complex(mpmath.fresnels(x)) for x in L]'
          'return A,' };
  c = pycall_sympy__ (cmd, num2cell (x(:)));
  y = reshape (cell2mat (c), size (x));
end

%!error fresnels (1, 2)

%!test
%! x = 1.1;
%! y = sym(11)/10;
%! A = fresnels (x);
%! B = double (fresnels (y));
%! assert (A, B, -4*eps);

%!test
%! y = [2 3 sym(pi); exp(sym(1)) 5 6];
%! x = double (y);
%! A = fresnels (x);
%! B = double (fresnels (y));
%! assert (A, B, -4*eps);

%!test
%! % maple:
%! % > A := [1+2*I, -2 + 5*I, 100, 10*I, -1e-4 + 1e-6*I, -20 + I];
%! % > for a in A do evalf(FresnelS(a)) end do;
%! x = [1+2i; -2+5i; 100; 10i; -1e-4 + 1e-6*1i; -20-1i];
%! A = [  36.725464883991438430 + 15.587751104404587334*1i
%!        0.12213736710980573217e13 - 0.47688568479924574726e12*1i
%!        0.49681690114783755327
%!       -0.46816997858488224040*1i
%!       -0.52344169596561937423e-12 + 0.15707439669173367248e-13*1i
%!        0.75738824160998910399e24 + 0.15391592966931193100e26*1i ];
%! B = fresnels (x);
%! assert (A, B, -eps)

%!assert (fresnels (inf), 0.5)
%!assert (fresnels (-inf), -0.5)


function out = FDSInit(startPos, endPos, map, scalling)

%% generate shape pattern
    radius = 10/scalling;
    mat = zeros(radius*2,radius*2);
    dista = @(a,b) sqrt(a*a+b*b);

    shapePattern = [];

    for x = 1:1:2*radius
        for y =1:1:2*radius
            if dista(x-radius-0.5,y-radius-0.5) > radius -1 && dista(x-radius-0.5,y-radius-0.5) < radius
                mat(x,y) = 1;
                shapePattern = [shapePattern; floor(x-radius) floor(y-radius) 0 0];
            end
        end
    end
    neighbours = [
        0 1 0 0;
        -1 0 0 0;
        0 -1 0 0;
        1 1 0 0;
        -1 -1 0 0;
        1 0 0 0;
        1 -1 0 0;
        -1 1 0 0;
    ]';

%% prepare all data

	out.map = map;
    out.startPos = [startPos(2:-1:1); 0; 0];
    out.endPos = [endPos(2:-1:1); 0; 0];
    startPos = out.startPos;

    out.sacalling = scalling;
    out.pattern = shapePattern';
    out.ucc = neighbours;
    out.height = ceil(length(out.map(:,1)));
    out.width = ceil(length(out.map(1,:)));
    out.graph = zeros(out.height, out.width,7);
    out.graph(:,:,1:2) = inf;
    out.graph(:,:,3) = -1;
    out.graph(:,:,5) = -1;
    out.graph(:,:,6) = 0;
    out.graph(:,:,7) = 0;
    out.kM = 0;
    SQRT2 = sqrt(2)-1;
    out.comparator = DStarcmp;
    out.stack = java.util.PriorityQueue(180247, out.comparator);

    setk(out.endPos, 0);
    seth(out.endPos, 0);
    setQ(out.endPos);
    out.endPos(3:4) = [heur(out.endPos); 0];
    add(out.stack, out.endPos);

    %-----------------------------------------------------------
    function setQ(s)
         out.graph(s(1), s(2), 3) = 1;
    end
    function seth(s, val)
        out.graph(s(1), s(2),1) = val;
    end
    function setk(s, val)
        out.graph(s(1), s(2),2) = val;
    end
    function out = heur(s)
        k = abs(startPos - s);
        out = SQRT2*min(k(1:2)) + max(k(1:2));
    end

end


function cvx_optpnt = exponential( sx )

%EXPONENTIAL   Exponential cone.
%   EXPONENTIAL, called with no arguments, creates three scalar variables X,
%   Y, and Z and constraints them to lie in an exponetial cone. That is,
%   given the declaration
%       variables x y z
%       {x,y,z} == exp_cone
%   constraints the variables to satisfy
%       y*exp(x/y) <= z
%       y > 0
%   The inequality form does not obey the disciplined convex programming
%   ruleset, but a function EXP_P has been created to represent this
%   computation; so the set declaration above is equivalent to
%       EXP_P(X,Y) <= Z
%   EXP_CONE(SX), where SX is a size vector, creates three array variables
%   X, Y, and Z, each of size SX, which are constrained elementwise to
%   satisfy EXP_P(X,Y) <= Z. If SX is empty, then SX=[1,1] is assumed.

cvx_expert_check( 'exponential' );
error( nargchk( 0, 1, nargin ) );

%
% Check size vector
%

if nargin == 0 || isempty( sx ),
    sx = [1,1]; %#ok
else
    [ temp, sx ] = cvx_check_dimlist( sx, true ); %#ok
    if ~temp,
        error( 'First argument must be a dimension vector.' );
    end
end

%
% Build the cone
%

cvx_begin_set
    variables x( sx ) y( sx ) z( sx )
    [ tx, dummy ] = find( cvx_basis( x ) ); %#ok
    [ ty, dummy ] = find( cvx_basis( y ) ); %#ok
    [ tz, dummy ] = find( cvx_basis( z ) ); %#ok
    newnonl( cvx_problem, 'exponential', [ tx(:)' ; ty(:)' ; tz(:)' ] );
cvx_end_set

cvx_optpnt = cvxtuple( struct( 'x', x, 'y', y, 'z', z ) );

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function z=state2RuvRR(xTar,useHalfRange,xTx,xRx,M,includeW)
%%STATE2RUVRR Convert state vectors consisting of at least 3D position and
%             velocity in 3D space into local bistatic r-u-v coordinates
%             with non-relativistic range rate.
%
%INPUTS: xTar An xDimXN matrix of N target states consisting of 3D position
%             and velocity components in the order
%             xTar=[xPosition;xVelocity] and possibly other components,
%             which will be ignored.
% useHalfRange A boolean value specifying whether the bistatic range value
%             should be divided by two. This normally comes up when
%             operating in monostatic mode, so that the range reported is a
%             one-way range. The default if this parameter is not provided
%             is false.
%         xTx An xTxDimXN matrix of the states of the transmitters
%             consisting of stacked 3D position and velocity components.
%             Other components will be ignored. If this parameter is
%             omitted, the transmitters are assumed to be stationary at the
%             origin. If only a single vector is passed, then the
%             transmitter state is assumed the same for all of the target
%             states being converted.
%         xRx An xRxDimXN matrix of the states of the receivers consisting
%             of stacked 3D position and velocity components. Other
%             components will be ignored. If this parameter is omitted, the
%             receivers are assumed to be stationary at the origin. If only
%             a single vector is passed, then the receiver state is assumed
%             the same for all of the target states being converted.
%           M A 3X3XN hypermatrix of the rotation matrices to go from the
%             alignment of the global coordinate system to that at the
%             receiver. The z-axis of the local coordinate system of the
%             receiver is the pointing direction of the receiver. If
%             omitted, then it is assumed that the local coordinate system
%             is aligned with the global and M=eye(3) --the identity matrix
%             is used. If only a single 3X3 matrix is passed, then is is
%             assumed to be the same for all of the N conversions.
%    includeW An optional boolean value indicating whether a third
%             direction cosine component should be included. The u and v
%             direction cosines are two parts of a 3D unit vector.
%             Generally, one might assume that the target is in front of
%             the sensor, so the third component would be positive and is
%             not needed. However, the third component can be included if
%             ambiguity exists. The default if this parameter is omitted or
%             an empty matrix is passed is false.
%
%OUTPUTS: z A 4XN (or 5XN if includeW is true) matrix of the target states
%           in xTar converted into bistatic r-u-v(-w)  and bistatic range
%           rate coordinates. If useHalfRange=true, then the r component is
%           half the bistatic range and the range rate is correspondingly
%           halved.
%
%Details of the conversions are given in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

N=size(xTar,2);

if(nargin<6||isempty(includeW))
   includeW=false;
end

if(nargin<5||isempty(M))
    M=repmat(eye(3),[1,1,N]);
elseif(size(M,3)==1&&N>1)
    M=repmat(M,[1,1,N]);
end

if(nargin<4||isempty(xRx))
    xRx=zeros(6,N);
elseif(size(xRx,2)==1&&N>1)
    xRx=repmat(xRx,[1,N]);
end

if(nargin<3||isempty(xTx))
    xTx=zeros(6,N);
elseif(size(xTx,2)==1&&N>1)
    xTx=repmat(xTx,[1,N]);
end

if(nargin<2||isempty(useHalfRange))
   useHalfRange=false;
end

%Allocate space for the converted states.
if(includeW)
    z=zeros(5,N);
else
    z=zeros(4,N);
end

%Convert the positions.
z(1:(end-1),:)=Cart2Ruv(xTar,useHalfRange,xTx,xRx,M,includeW);

%Compute the bistatic range rates.
z(end,:)=getRangeRate(xTar(1:6,:),useHalfRange,xTx,xRx);
end

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


function segclassifier = mcmcTrainSegmentationClassifier(data, labels, segimind, trainind)

ntrees = 10;
nnodes = 8;

disp(num2str(mean(labels==1)))

keep = zeros(numel(labels), 1);

for k = 1:numel(trainind)
    keep(find(segimind==trainind(k))) = 1;
end
ind = find(keep);

segclassifier = train_boosted_dt_2c(data(ind, :), [], labels(ind), ntrees, nnodes);


% Sliding Window Gauss Newton Optimization
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clc
clear
close all
load('../KITTI Trials/SWF_2011_09_26_drive_0036_sync_KLT.mat');
k1 = kStart;
k2 = k1+kappa;
K = k2 - k1;  %There are K + 1 total states, since x0 is the k1th state
% v_var = 0.1*ones(3,1);
% w_var = 0.1*ones(3,1);
% y_var = 0.5^2*ones(2,1);                 % pixel coord var
% JcostThresh = 0.1e-2;

%% Setup
if useMonoCamera
    pixMeasDim = 2;
else
    pixMeasDim = 4;
end

initialStateStruct = {};

% Extract noise values
Q = diag([v_var; w_var]);
if useMonoCamera
    R = diag(y_var);
    %R = diag(y_var(1:2));
else
    R = 1*eye(4);
    %R = diag(y_var);
end

%% First create the initial guess using dead reackoning

%Use ground truth for the first state
firstState.C_vi = Cfrompsi(theta_vk_i(:,k1));
if isnan(firstState.C_vi(1,1))
    firstState.C_vi = eye(3);
end
firstState.r_vi_i = r_i_vk_i(:,k1);
firstState.k = k1;
initialStateStruct{1} = firstState;

%There are K + 1 states (there is a '0' state)
for kIdx = 1:K
    k = kIdx + k1;
    imuMeasurement.omega = w_vk_vk_i(:, k-1);
    imuMeasurement.v = v_vk_vk_i(:, k-1);
    deltaT = t(k) - t(k-1);
    %Propagate the state forward
    initialStateStruct{kIdx+1} = propagateState(initialStateStruct{kIdx}, imuMeasurement, deltaT);
end

%% IMU Only
imuOnlyStateStruct{1} = firstState;
%There are K + 1 states (there is a '0' state)
for k = kStart+1:kEnd+1
    imuMeasurement.omega = w_vk_vk_i(:, k-1);
    imuMeasurement.v = v_vk_vk_i(:, k-1);
    deltaT = t(k) - t(k-1);
    %Propagate the state forward
    imuOnlyStateStruct{k} = propagateState(imuOnlyStateStruct{k-1}, imuMeasurement, deltaT);
end

%%

%Slide the window along
stateVecHistStruct = {};
stateVecHistStruct{1} = firstState;

%stateSigmaHistMat = [];
figure
plot(-firstState.r_vi_i(2),firstState.r_vi_i(1), '*b')
hold on;
grid on;
for k1 = kStart:kEnd
k2 = k1+kappa;

% %How many exteroceptive measurements do we have?
% %NOTE: k1 is the 0th state
% totalLandmarkObs = 0;
% observedBinaryFlags = zeros(numLandmarks, 1);
% lmObsVec = zeros(1, K);
%
% for k = (k1+1):k2
%     validObs = squeeze(y_k_j(1, k, :) > -1);
%     lmObsVec(k-k1) = sum(validObs);
%     observedBinaryFlags(validObs') = ones(1, sum(validObs==1));
%     totalLandmarkObs = totalLandmarkObs + sum(y_k_j(1, k, :) > -1);
% end
% totalUniqueObservedLandmarks = sum(observedBinaryFlags);
% observedLandmarkIds = find(observedBinaryFlags);

%To initialize G-N, we propagate all the states for the first window
%and then only propagate the most recent state, re-using the rest
if k1 == kStart
        currentStateStruct = initialStateStruct;
        rho_i_pj_i_est = NaN(3, numLandmarks);
else
        currentStateStruct = currentStateStruct(2:end);

        %Extract the measurement
        imuMeasurement.omega = w_vk_vk_i(:, k2-1);
        imuMeasurement.v = v_vk_vk_i(:, k2-1);
        deltaT = t(k2) - t(k2-1);

        %Propagate the state forward
        currentStateStruct{end+1} = propagateState(currentStateStruct{end}, imuMeasurement, deltaT);
end

% Initialize the landmark positions (start fresh each window)
rho_i_pj_i_est = NaN(3, numLandmarks);
observedLandmarkStructs = {};
for l_i = 1:numLandmarks
       observedLandmarkStructs{l_i}.camStates = {};
       observedLandmarkStructs{l_i}.observations = [];
end
for kIdx = 1:K
        k = kIdx + k1;
        validLmObsId = find(y_k_j(1, k, :) > -1);
        kState = currentStateStruct{kIdx+1};
        T_vi = [kState.C_vi -kState.C_vi*kState.r_vi_i; 0 0 0 1];
        T_ci = T_cv*T_vi;
        T_ic = inv(T_ci);

        % camStates{k}.q_CG        4x1 Global to camera rotation quaternion
        % camStates{k}.p_C_G       3x1 Camera Position in the Global frame
        % camStates{k}.trackedFeatureIds  1xM List of feature ids that are currently being tracked from that camera state

        for lmId = validLmObsId'

            yMeas = y_k_j(:, k, lmId);
            camState = {};
            camState.C_CG = T_ci(1:3,1:3);
            camState.p_C_G = T_ic(1:3,4);
            observedLandmarkStructs{lmId}.camStates{end+1} = camState;
            observedLandmarkStructs{lmId}.observations(:, end+1) = [(yMeas(1) - calibParams.c_u)/calibParams.f_u; (yMeas(2) - calibParams.c_v)/calibParams.f_v];

            %Find the ground truth position of the observed landmark
            %rho_pi_i_check = rho_i_pj_i(:, lmId);

%             if (isnan(rho_i_pj_i_est(1, lmId)))
%                 %Use triangulation to find the position of the landmark
%                  rho_pc_c = triangulate(yMeas, calibParams);
%
%                  rho_pi_i = kState.C_vi'*(vehicleCamTransform.C_cv'*rho_pc_c + vehicleCamTransform.rho_cv_v) +  kState.r_vi_i;
%                  rho_i_pj_i_est(:, lmId) = rho_pi_i;
%
%                  %Use ground truth for now
%                  %rho_i_pj_i_est(:, lmId) = rho_i_pj_i(:, lmId);
%             end
        end
end

%Triangulate all landmarks and keep track of which ones we are
%triangulating
totalLandmarkObs = 0;
observedLandmarkIds = [];
totalUniqueObservedLandmarks = 0;
for lmId = 1:length(observedLandmarkStructs)
    if length(observedLandmarkStructs{lmId}.camStates) > 1
        camStates = observedLandmarkStructs{lmId}.camStates;
        observations = observedLandmarkStructs{lmId}.observations;
        [rho_pi_i, Jcost, RCOND] = calcGNPosEst(camStates, observations, noiseParams);
        if Jcost < JcostThresh*length(camStates)^2
            rho_i_pj_i_est(:, lmId) = rho_pi_i;
            totalLandmarkObs = totalLandmarkObs + length(observedLandmarkStructs{lmId}.camStates);
            totalUniqueObservedLandmarks = totalUniqueObservedLandmarks + 1;
            observedLandmarkIds(end+1) = lmId;
        end
    end
end

%Define the optimal state
optimalStateStruct = currentStateStruct;
Jbest = Inf;
dx = Inf;

for optIdx = 1:maxOptIter+1

%Error Vector
errorVector = NaN(6*K+pixMeasDim*totalLandmarkObs, 1);
%This helper index will keep track of where we need to insert our next
%errors
errorVectorHelperIdx = 1;

%H and T
H = sparse(6*K+pixMeasDim*totalLandmarkObs, 6*(K+1) + 3*totalUniqueObservedLandmarks);
T = sparse(6*K+pixMeasDim*totalLandmarkObs, 6*K+pixMeasDim*totalLandmarkObs);

%Helper indices that keep track of the row number of the last block entry
%into H and T
HHelperIdx = 1;
THelperIdx = 1;

for kIdx = 1:K
    k = kIdx + k1;
    imuMeasurement.omega = w_vk_vk_i(:, k-1);
    imuMeasurement.v = v_vk_vk_i(:, k-1);
    deltaT = t(k) - t(k-1);

    %==== Build the interoceptive error and Jacobians=====%
    %Note that there are K+1 states (the 0th state is the 1st element)
    kState = currentStateStruct{kIdx+1};
    kMinus1State = currentStateStruct{kIdx};

    intErrorVec = imuError(kState, kMinus1State, imuMeasurement, deltaT);
    H_x_k =  H_xfn(kMinus1State, imuMeasurement, deltaT );
    H_w_k = H_wfn(kMinus1State);

    %==== Build the exteroceptive error and Jacobians=====%
    validLmObsId = intersect(find(y_k_j(1, k, :) > -1), observedLandmarkIds);

    if kIdx == 1 && optIdx == 1
        fprintf('Tracking %d features. \n', length(validLmObsId));
    end

    if ~isempty(validLmObsId)

        extErrorVec = NaN(pixMeasDim*length(validLmObsId), 1);
        G_x_k = NaN(pixMeasDim*length(validLmObsId),6);
        %Jacobians wrt feature position
        G_x_f_k = NaN(pixMeasDim*length(validLmObsId), 3);

        idx = 1;
        for lmId = validLmObsId'

            yMeas = y_k_j(:, k, lmId);

            rho_pi_i = rho_i_pj_i_est(:,lmId);

            stereoError = stereoCamError(yMeas, kState, vehicleCamTransform, rho_pi_i, calibParams);

            [G_x_k_state, G_x_k_feat] = G_xfn(kState, vehicleCamTransform, rho_pi_i, calibParams, useMonoCamera);

            %Use stereo or monocular errors
            if useMonoCamera
                extErrorVec(idx:idx+1, 1) = stereoError(1:2);

                G_x_k(idx:idx+1, :) = G_x_k_state;
                G_x_f_k(idx:idx+1, :) = G_x_k_feat;
                idx = idx + 2;
            else
                extErrorVec(idx:idx+3, 1) = stereoError;

                G_x_k(idx:idx+3, :) = G_x_k_state;
                G_x_f_k(idx:idx+3, :) = G_x_k_feat;
                idx = idx + 4;
            end
        end
    else
        extErrorVec = [];
    end

    %Update matrices
    %==== Error vector =====
    combinedErrorVec = [intErrorVec; extErrorVec];
    errorVector(errorVectorHelperIdx:(errorVectorHelperIdx + length(combinedErrorVec) - 1) ,1) = combinedErrorVec;
    errorVectorHelperIdx = errorVectorHelperIdx + length(combinedErrorVec);

    %==== H matrix =====
    Hblock = zeros(6+pixMeasDim*length(validLmObsId), 12);
    Hblock(1:6,1:6) = -H_x_k;
    Hblock(1:6,7:12) = eye(6);

    if ~isempty(validLmObsId)
        Hblock(7:(7+pixMeasDim*length(validLmObsId) - 1), 7:12) = -G_x_k;
    end
    Hblockrows = size(Hblock, 1);

    H(HHelperIdx:(HHelperIdx + Hblockrows - 1), 1+6*(kIdx-1):12+6*(kIdx-1) ) = Hblock;

    %Add the feature Jacobians
    lmNum = 1;
    for lmId = validLmObsId'
        rowIdx = pixMeasDim*(lmNum-1)+1;

        colLmId = find(observedLandmarkIds == lmId);
        colIdx = 6*(K+1)+3*colLmId-2;

        H(HHelperIdx+5+rowIdx:HHelperIdx+rowIdx+5+(pixMeasDim-1), colIdx:colIdx+2) = -G_x_f_k(rowIdx:rowIdx+pixMeasDim-1, :);
        lmNum = lmNum + 1;
    end

    HHelperIdx = HHelperIdx + Hblockrows;

    %==== T matrix =====
    T_k = zeros(6+pixMeasDim*length(validLmObsId), 6+pixMeasDim*length(validLmObsId));
    T_k(1:6, 1:6) = H_w_k*Q*deltaT^2*H_w_k';
    %Here, G_n_k is identity, so we can just repeat the variances along the
    %diagonal
    obsVar = diag(R);
    T_k(7:end, 7:end) = diag(repmat(obsVar, [length(validLmObsId),1]));
    Tksize = size(T_k, 1);
    T(THelperIdx:(THelperIdx + Tksize - 1), THelperIdx:(THelperIdx + Tksize - 1)) = T_k;
    THelperIdx = THelperIdx + Tksize;
end

    H = H(:, 7:end);

    %Calculate scalar objective
    Jnew = 0.5*errorVector'*(T\errorVector);

    if Jnew < Jbest
        optimalStateStruct = currentStateStruct;
        Jbest = Jnew;
    end

    %Check for convergence
    if norm(dx) < 1e-3
        disp('Converged!')
        break;
    end

    % Solve for the optimal step size!
    if optIdx <= maxOptIter
        dx = (H'*(T\H) + LMLambda*diag(diag(H'*(T\H))))\(-H'*(T\errorVector));
        [currentStateStruct, rho_i_pj_i_est] = updateStateStruct(currentStateStruct, observedLandmarkIds, rho_i_pj_i_est,  lineLambda*dx);
    end

end %End optimization iterations

% error = 0;
% for i=1:numLandmarks
%     if ~isnan(rho_i_pj_i_est(1,i))
%         error = error + norm(rho_i_pj_i_est(:,i) - rho_i_pj_i(:,i));
%     end
% end
% error = error/numLandmarks
currentStateStruct = optimalStateStruct;

if optIdx == maxOptIter
    fprintf('Warning: Failed to converge! \n');
end

fprintf('%d done. J = %.5f. %d iterations. \n', k1, Jbest, optIdx)

%Extract variance of states
stateCov = inv(H'*(T\H) + LMLambda*eye(size(H,2)));
stateVar = diag(stateCov);

%Keep track of the first state in the window
% if ~all(stateVar > 0)
%     warning('Variances not positive');
% end
stateVecHistStruct{end+1} = currentStateStruct{2};
plot(-currentStateStruct{2}.r_vi_i(2),currentStateStruct{2}.r_vi_i(1), '*b')
plot(-imuOnlyStateStruct{k1}.r_vi_i(2),imuOnlyStateStruct{k1}.r_vi_i(1), '*r')
plot(-r_i_vk_i(2,k1),r_i_vk_i(1,k1), '*k');

drawnow;
stateSigmaHistMat(:,k1 - kStart + 1) = sqrt(abs(stateVar(1:6)));

end %End Sliding window

toc

sigma_x = (stateSigmaHistMat(1,:));
sigma_y = (stateSigmaHistMat(2,:));
sigma_z = (stateSigmaHistMat(3,:));
sigma_th1 = (stateSigmaHistMat(4,:));
sigma_th2 = (stateSigmaHistMat(5,:));
sigma_th3 = (stateSigmaHistMat(6,:));

%% Plot error and variances
%addpath('/Users/valentinp/Research/MATLAB/export_fig'); %Use Oliver Woodford's awesome export_fig package to get trimmed PDFs
addpath('../msckf/utils')
rotErrVec = zeros(3, length(stateVecHistStruct));
transErrVec = zeros(3, length(stateVecHistStruct));
transErrVecImu = zeros(3, length(stateVecHistStruct));
rotErrVecImu = zeros(3, length(stateVecHistStruct));

translation = zeros(3, length(stateVecHistStruct));
translation_imuonly = zeros(3, length(stateVecHistStruct));

for stIdx = 1:length(stateVecHistStruct)
    translation(:, stIdx) = stateVecHistStruct{stIdx}.r_vi_i;
    translation_imuonly(:,stIdx) = imuOnlyStateStruct{stIdx}.r_vi_i;
    transErrVec(:,stIdx) = stateVecHistStruct{stIdx}.r_vi_i - r_i_vk_i(:,stIdx);
    transErrVecImu(:,stIdx) = imuOnlyStateStruct{stIdx}.r_vi_i - r_i_vk_i(:,stIdx);
    eRotMat = eye(3) - stateVecHistStruct{stIdx}.C_vi*axisAngleToRotMat(theta_vk_i(:,stIdx))';
    rotErrVec(:, stIdx) = [eRotMat(3,2); eRotMat(1,3); eRotMat(2,1)];

    eRotMat = eye(3) - imuOnlyStateStruct{stIdx}.C_vi*axisAngleToRotMat(theta_vk_i(:,stIdx))';
    rotErrVecImu(:, stIdx) = [eRotMat(3,2); eRotMat(1,3); eRotMat(2,1)];
end

figure
plot3(-translation(2,:),translation(1,:),translation(3,:), '-b');
hold on
plot3(-translation_imuonly(2,:),translation_imuonly(1,:),translation_imuonly(3,:), '-r');
plot3(-r_i_vk_i(2,1:length(stateVecHistStruct)),r_i_vk_i(1,1:length(stateVecHistStruct)),r_i_vk_i(3,1:length(stateVecHistStruct)), '-k');
legend('Opt', 'IMU', 'Ground Truth')
armse_imu = mean(sqrt(sum(transErrVecImu.^2,1)/3))
armse_imu_rot = mean(sqrt(sum(rotErrVecImu(:,2:end).^2,1)/3))

armse = mean(sqrt(sum(transErrVec.^2,1)/3))
armse_rot = mean(sqrt(sum(rotErrVec(:,2:end).^2,1)/3))

norm(transErrVecImu(:,end))
norm(transErrVec(:,end))

save(sprintf('../KITTI Trials/SWF_RERUN_%s', fileName));
%%
% numFeat = NaN(1, 100);
% for f_i = 1:100
%    numFeat(f_i) = sum(y_k_j(1,f_i,:) > -1);
% end
% plot(numFeat)
% Save estimates
% swf_trans_err = transErrVec;
% swf_rot_err = rotErrVec;
% save(sprintf('swf_%d_%d_%d_%s.mat',kStart,kEnd, kappa, fileName), 'swf_trans_err', 'swf_rot_err', 'stateSigmaHistMat');

% transLim = 0.5;
% rotLim = 0.5;
% recycleStates = 'Yes';
%
% figure
% subplot(3,1,1)
% plot(t(kStart:kEnd), transErrVec(1,:), 'LineWidth', 1.2)
% hold on
% plot(t(kStart:kEnd), 3*sigma_x, '--r')
% plot(t(kStart:kEnd), -3*sigma_x, '--r')
% ylim([-transLim transLim])
% title(sprintf('Translational Error (\\kappa = %d, Recycle States? %s)', kappa, recycleStates))
% ylabel('\delta r_x')
%
%
% subplot(3,1,2)
% plot(t(kStart:kEnd), transErrVec(2,:), 'LineWidth', 1.2)
% hold on
% plot(t(kStart:kEnd), 3*sigma_y, '--r')
% plot(t(kStart:kEnd), -3*sigma_y, '--r')
% ylim([-transLim transLim])
% ylabel('\delta r_y')
%
% subplot(3,1,3)
% plot(t(kStart:kEnd), transErrVec(3,:), 'LineWidth', 1.2)
% hold on
% plot(t(kStart:kEnd), 3*sigma_z, '--r')
% plot(t(kStart:kEnd), -3*sigma_z, '--r')
% ylim([-transLim transLim])
% ylabel('\delta r_z')
% xlabel('t_k')
% %set(gca,'FontSize',12)
% %set(findall(gcf,'type','text'),'FontSize',12)
%
% figure
% subplot(3,1,1)
% plot(t(kStart:kEnd), rotErrVec(1,:), 'LineWidth', 1.2)
% hold on
% plot(t(kStart:kEnd), 3*sigma_th1, '--r')
% plot(t(kStart:kEnd), -3*sigma_th1, '--r')
% ylim([-rotLim rotLim])
% title(sprintf('Rotational Error (\\kappa = %d, Recycle States? %s)', kappa, recycleStates))
% ylabel('\delta\theta_x')
%
%
% subplot(3,1,2)
% plot(t(kStart:kEnd), rotErrVec(2,:), 'LineWidth', 1.2)
% hold on
% plot(t(kStart:kEnd), 3*sigma_th2, '--r')
% plot(t(kStart:kEnd), -3*sigma_th2, '--r')
% ylim([-rotLim rotLim])
% ylabel('\delta\theta_y')
%
% subplot(3,1,3)
% plot(t(kStart:kEnd), rotErrVec(3,:), 'LineWidth', 1.2)
% hold on
% plot(t(kStart:kEnd), 3*sigma_th3, '--r')
% plot(t(kStart:kEnd), -3*sigma_th3, '--r')
% ylim([-rotLim rotLim])
% ylabel('\delta\theta_z')
% xlabel('t_k')


function [p,f] = spm_powell(p,xi,tolsc,func,varargin)
% Powell optimisation method
% FORMAT [p,f] = spm_powell(p,xi,tolsc,func,varargin)
%   p        - Starting parameter values
%   xi       - columns containing directions in which to begin searching
%   tolsc    - stopping criteria, optimisation stops when
%                sqrt(sum(((p-p_prev)./tolsc).^2))<1
%   func     - name of evaluated function
%   varargin - remaining arguments to func (after p)
%
%   p        - final parameter estimates
%   f        - function value at minimum
%__________________________________________________________________________
%
% Method is based on Powell's optimisation method described in
% Numerical Recipes (Press, Flannery, Teukolsky & Vetterling).
%__________________________________________________________________________
% Copyright (C) 2001-2017 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_powell.m 7252 2018-01-31 15:56:56Z john $

p = p(:);
f = feval(func,p,varargin{:});
for iter=1:512
    %if numel(p)>1, fprintf('iteration %d...\n', iter); end;            %-#
    ibig = numel(p);
    pp   = p;
    fp   = f;
    del  = 0;
    for i=1:length(p)
        ft = f;
        [p,junk,f] = min1d(p,xi(:,i),func,f,tolsc,varargin{:});
        if abs(ft-f) > del
            del  = abs(ft-f);
            ibig = i;
        end
    end
    if numel(p)==1 || sqrt(sum(((p(:)-pp(:))./tolsc(:)).^2))<1 || abs((f-fp)/(f+fp))<1e-6, return; end
    ft = feval(func,2.0*p-pp,varargin{:});
    if ft < f
        [p,xi(:,ibig),f] = min1d(p,p-pp,func,f,tolsc,varargin{:});
    end
end
warning('Too many optimisation iterations');

%==========================================================================
% function [p,pi,f] = min1d(p,pi,func,f,tolsc,varargin)
%==========================================================================
function [p,pi,f] = min1d(p,pi,func,f,tolsc,varargin)
% Line search for minimum.

global lnm % used in funeval
lnm      = struct('p',p,'pi',pi,'func',func,'args',[]);
lnm.args = varargin;

min1d_plot('Init', 'Line Minimisation','Function','Parameter Value');
min1d_plot('Set', 0, f);

tol      = 1/sqrt(sum((pi(:)./tolsc(:)).^2));
t        = bracket(f);
[f,pmin] = search(t,tol);
pi       = pi*pmin;
p        = p + pi;

%if length(p)<12,
%   for i=1:length(p), fprintf('%-8.4g ', p(i)); end;                  %-#
%   fprintf('| %.5g\n', f);                                            %-#
%else
%    fprintf('%.5g\n', f);                                              %-#
%end
min1d_plot('Clear');

%==========================================================================
% function f = funeval(p)
%==========================================================================
function f = funeval(p)
% Reconstruct parameters and evaluate.

global lnm % defined in min1d
pt = lnm.p+p.*lnm.pi;
f  = feval(lnm.func,pt,lnm.args{:});
min1d_plot('Set',p,f);

%==========================================================================
% function t = bracket(f)
%==========================================================================
function t = bracket(f)
% Bracket the minimum (t(2)) between t(1) and t(3)

gold   = (1+sqrt(5))/2; % Golden ratio

t(1)   = struct('p',0,'f',f);
t(2).p = 1;
t(2).f = funeval(t(2).p);

% if t(2) not better than t(1) then swap
if t(2).f > t(1).f
    t(3) = t(1);
    t(1) = t(2);
    t(2) = t(3);
end

t(3).p = t(2).p + gold*(t(2).p-t(1).p);
t(3).f = funeval(t(3).p);

while t(2).f > t(3).f

    % fit a polynomial to t
    tmp = cat(1,t.p)-t(2).p;
    pol = pinv([ones(3,1) tmp tmp.^2])*cat(1,t.f);

    % minimum is when gradient of polynomial is zero
    % sign of pol(3) (the 2nd deriv) should be +ve
    if pol(3)>0
        % minimum is when gradient of polynomial is zero
        d    = -pol(2)/(2*pol(3)+eps);

        % A very conservative constraint on the displacement
        if d > (1+gold)*(t(3).p-t(2).p)
            d = (1+gold)*(t(3).p-t(2).p);
        end
        u.p  = t(2).p+d;
    else
        % sign of pol(3) (the 2nd deriv) is not +ve
        % so extend out by golden ratio instead
        u.p  = t(3).p+gold*(t(3).p-t(2).p);
    end

    % FUNCTION EVALUATION
    u.f  = funeval(u.p);

    if (t(2).p < u.p) == (u.p < t(3).p)

        % u is between t(2) and t(3)
        if u.f < t(3).f
            % minimum between t(2) and t(3) - done
            t(1) = t(2);
            t(2) = u;
            return
        elseif u.f > t(2).f
            % minimum between t(1) and u - done
            t(3) = u;
            return;
        end
    end

    % Move all 3 points along
    t(1) = t(2);
    t(2) = t(3);
    t(3) = u;
end

%==========================================================================
% function [f,p] = search(t, tol)
%==========================================================================
function [f,p] = search(t, tol)
% Brent's method for line searching - given that minimum is bracketed

gold1 = 1-(sqrt(5)-1)/2;

% Current and previous displacements
d     = Inf;
pd    = Inf;

% sort t into best first order
[junk,ind] = sort(cat(1,t.f));
t   = t(ind);
brk = [min(cat(1,t.p)) max(cat(1,t.p))];

for iter=1:128
    % check stopping criterion
    if abs(t(1).p - 0.5*(brk(1)+brk(2)))+0.5*(brk(2)-brk(1)) <= 2*tol
        p = t(1).p;
        f = t(1).f;
        return;
    end

    % keep last two displacents
    ppd = pd;
    pd  = d;

    % fit a polynomial to t
    tmp = cat(1,t.p)-t(1).p;
    pol = pinv([ones(3,1) tmp tmp.^2])*cat(1,t.f);

    % minimum is when gradient of polynomial is zero
    d   = -pol(2)/(2*pol(3)+eps);
    u.p = t(1).p+d;

    % check so that displacement is less than the last but two,
    % that the displaced point is between the brackets
    % and that the solution is a minimum rather than a maximum
    eps2 = 2*eps*abs(t(1).p)+eps;
    if abs(d) > abs(ppd)/2 || u.p < brk(1)+eps2 || u.p > brk(2)-eps2 || pol(3)<=0
        % if criteria are not met, then golden search into the larger part
        if t(1).p >= 0.5*(brk(1)+brk(2))
            d = gold1*(brk(1)-t(1).p);
        else
            d = gold1*(brk(2)-t(1).p);
        end
        u.p = t(1).p+d;
    end

    % FUNCTION EVALUATION
    u.f = funeval(u.p);

    % Insert the new point into the appropriate position and update
    % the brackets if necessary
    if u.f <= t(1).f
        if u.p >= t(1).p, brk(1)=t(1).p; else brk(2)=t(1).p; end
        t(3) = t(2);
        t(2) = t(1);
        t(1) = u;
    else
        if u.p < t(1).p, brk(1)=u.p; else brk(2)=u.p; end
        if u.f <= t(2).f
            t(3) = t(2);
            t(2) = u;
        elseif u.f <= t(3).f
            t(3) = u;
        end
    end
end

%==========================================================================
% function min1d_plot(action,arg1,arg2,arg3)
%==========================================================================
function min1d_plot(action,arg1,arg2,arg3)
% Visual output for line minimisation
persistent min1dplot

if ~nargin, action = 'Init'; end

% Find the Interactive window and exit if not
%--------------------------------------------------------------------------
fg = spm_figure('FindWin','Interactive');
if isempty(fg), return; end

%-Initialize
%--------------------------------------------------------------------------
if strcmpi(action,'init')
    if nargin<4, arg3 = 'Function';          end
    if nargin<3, arg2 = 'Value';             end
    if nargin<2, arg1 = 'Line minimisation'; end

    min1dplot = struct('pointer',get(fg,'Pointer'),...
                       'name',   get(fg,'Name'),...
                       'ax',     []);
    min1d_plot('Clear');
    set(fg,'Pointer','Watch');
    min1dplot.ax = axes('Position', [0.15 0.1 0.8 0.75],...
                        'Box',      'on',...
                        'Parent',   fg);
    lab = get(min1dplot.ax,'Xlabel');
    set(lab,'string',arg3,'FontSize',10);
    lab = get(min1dplot.ax,'Ylabel');
    set(lab,'string',arg2,'FontSize',10);
    lab = get(min1dplot.ax,'Title');
    set(lab,'string',arg1);
    line('Xdata',[], 'Ydata',[],...
        'LineWidth',2,'Tag','LinMinPlot',...
        'LineStyle','-','Marker','o',...
        'Parent',min1dplot.ax);
    drawnow;

%-Reset
%--------------------------------------------------------------------------
elseif strcmpi(action,'set')
    br = findobj(fg,'Tag','LinMinPlot');
    if ~isempty(br)
        [xd,indx] = sort([get(br,'Xdata') arg1]);
        yd = [get(br,'Ydata') arg2];
        yd = yd(indx);
        set(br,'Ydata',yd,'Xdata',xd);
        drawnow;
    end

%-Clear
%--------------------------------------------------------------------------
elseif strcmpi(action,'clear')
    fg = spm_figure('FindWin','Interactive');
    if isstruct(min1dplot)
        if ishandle(min1dplot.ax), delete(min1dplot.ax); end
        set(fg,'Pointer',min1dplot.pointer);
        set(fg,'Name',min1dplot.name);
    end
    spm_figure('Clear',fg);
    drawnow;
end


function [ceq, ceqGrad] = cst_heelStrike(x0,xF,p)
% [ceq, ceqGrad] = cst_heelStrike(x0,xF,p)
%
% This function computes the heel-strike constraint as well as gradients.
%
% INPUTS:
%   t0 = time at the start of the trajectory
%   x0 = state at the start of the trajectory
%   tF = time at the end of the trajectory
%   xF = state at the end of the trajectory
%
% OUTPUTS:
%   ceq = equality constraint
%   ceqGrad = gradient of equality constraints
%

qm = xF(1:5);
qp = x0(1:5);
dqm = xF(6:10);
dqp = x0(6:10);

if nargout == 1   %numerical gradients
    [m,mi,f,fi] = autoGen_cst_heelStrike(...
        qp(1),qp(2),qp(3),qp(4),qp(5),...  %Angles "plus" - immediately after heel strike
        qm(1),qm(2),qm(3),qm(4),qm(5),...  %Angles "minus" - immediately before heel-strike
        dqm(1),dqm(2),dqm(3),dqm(4),dqm(5),...   %Rates "minus" - immediately before heel-strike
        p.m1, p.m2, p.m3, p.m4, p.m5, p.I1, p.I2, p.I3, p.I4, p.I5, p.l1, p.l2, p.l3, p.l4, p.l5, p.c1, p.c2, p.c3, p.c4, p.c5, 0);

    M = zeros(5,5);  %Mass matrix
    F = zeros(5,1);
    M(mi) = m(:,1);
    F(fi) = f(:,1);
    dqpDyn = M\F;  %Numerically invert the mass matrix

    ceqPos = qp - qm([5;4;3;2;1]);
    ceqVel = dqpDyn - dqp;
    ceq = [ceqPos;ceqVel];

else %Analytic gradients

    [m,mi,f,fi,mz,mzi,mzd,fz,fzi,fzd] = autoGen_cst_heelStrike(...
        qp(1),qp(2),qp(3),qp(4),qp(5),...  %Angles "plus" - immediately after heel strike
        qm(1),qm(2),qm(3),qm(4),qm(5),...  %Angles "minus" - immediately before heel-strike
        dqm(1),dqm(2),dqm(3),dqm(4),dqm(5),...   %Rates "minus" - immediately before heel-strike
        p.m1, p.m2, p.m3, p.m4, p.m5, p.I1, p.I2, p.I3, p.I4, p.I5, p.l1, p.l2, p.l3, p.l4, p.l5, p.c1, p.c2, p.c3, p.c4, p.c5, 0);

    M = zeros(5,5);  %Mass matrix
    F = zeros(5,1);
    M(mi) = m(:,1);
    F(fi) = f(:,1);
    dqpDyn = M\F;  %Numerically invert the mass matrix

    Mz = zeros(mzd);
    Fz = zeros(fzd);
    Mz(mzi) = mz;
    Fz(fzi) = fz;

    nz = 22;   %Number of dimensions for gradients [t0;x0;tF;xF]
    dqpDynGrad = zeros(5,nz);
    for j=1:nz
        dqpDynGrad(:,j) = M\( -Mz(:,:,j)*dqpDyn + Fz(:,:,j) );  % Derivative of a matrix inverse http://www.atmos.washington.edu/~dennis/MatrixCalculus.pdf
    end

    dqpGrad = zeros(5,nz);
    qpGrad = zeros(5,nz);
    qmGrad = zeros(5,nz);
    for i=1:5
        qpGrad(i,1+i) = 1;
        dqpGrad(i,6+i) = 1;
        qmGrad(i,12+i) = 1;
    end

    ceqPos = qp - qm([5;4;3;2;1]);
    ceqVel = dqpDyn - dqp;
    ceq = [ceqPos;ceqVel];

    ceqPosGrad = qpGrad - qmGrad([5;4;3;2;1],:);
    ceqVelGrad = dqpDynGrad - dqpGrad;
    ceqGrad = [ceqPosGrad;ceqVelGrad];

end

end


function b=BlockWise(img,level,flag)

R_mean = mean(mean(img(:,:,1)));
G_mean = mean(mean(img(:,:,2)));
B_mean = mean(mean(img(:,:,3)));
W = size(img,1);
H = size(img,2);

switch flag
    case 5
        block = repmat([R_mean G_mean B_mean]',[1,32,32]);
        block = permute(block,[3,2,1]);
    case 4
        block = repmat([R_mean G_mean B_mean]',[1,32,32]);
        block = permute(block,[3,2,1]);
    case 3
        block = repmat([R_mean+30 G_mean B_mean]',[1,32,32]);
        block = permute(block,[3,2,1]);
    case 2
        block = repmat([R_mean+50 G_mean B_mean]',[1,32,32]);
        block = permute(block,[3,2,1]);
    case 1
        block = repmat([0 G_mean B_mean]',[1,32,32]);
        block = permute(block,[3,2,1]);
end

block = uint8(block);

for i=1:level
    r_W = randi([1 W-32],1);
    r_H = randi([2 H-32],1);
    img(r_W:r_W+31,r_H:r_H+31,:) = block;
end

b= img;

end


clear
        % Incarcarea datelor din tabel
I=0:0.5:5;
x=linspace(0,pi,length(I));
for k=1:length(I)
    yy=3*I(k)*sin(x);
    y(:,k)=yy';
end
        % Incarcarea tabelului de cautare
tab=zeros(12);
tab(2:12,1)=x';
tab(1,2:12)=I;
tab(2:12,2:12)=y;
        % Cautarea in tabel
a=table2(tab,pi/10,1)
b=table2(tab,0.5,0.75)


function [thr, h1] = badacostCalibrateCascade(X0, X1, clf, use_trees)
% Classify all the data with the classifier and check the
% costs traces. We compare positive class trace (label 1) with
% the min cost of the positive classes. We use the formula:
%
%  traces(i,:) = -(min_pos_costs - costs(1,:));
%
%  Where traces(i, :) are the traces for the i-th badacost weak learner,
%  costs(1,:) are the costs associated with the negative class at
%  i-th badacost weak-learner and min_pos_cost is the min cost at the
%  i-th weak-learner of the positive classes.

if nargin < 4
  use_trees = 1;
end

% 1. Compute costs traces ...
if use_trees
  nWeaks = size(clf.fids, 2);
else
  nWeaks = length(clf.WEAK_LEARNERS);
end
n0 = size(X0, 1);
n1 = size(X1, 1);
%n  = n0 + n1;
margin_vec0 = zeros(clf.num_classes, n0);
margin_vec1 = zeros(clf.num_classes, n1);
traces0     = zeros(nWeaks, n0);
traces1     = zeros(nWeaks, n1);

for i=1:nWeaks
  if use_trees
    % z is a row vector with the labels
    ids0 = forestInds(X0,clf.thrs(:,i),clf.fids(:,i),clf.child(:,i), 1e5);
    for j=1:n0
      z = clf.hs(ids0(j),i);
      margin_vec0(:,j) = margin_vec0(:,j) + ...
                        (clf.wl_weights(i).*clf.Y(:, z));
    end
  else % General case with other weak learners
    % z is a row vector with the labels
    z  = clf.classify_weak_learner(clf.WEAK_LEARNERS{i}, X0');
    for j=1:n0
      margin_vec0(:,j) = margin_vec0(:,j) + (clf.WEIGHTS(i).*clf.Y(:, z(j)));
    end
  end
  % WARNING: Change to accomodate with theory (2016/11)
%  costs = clf.Cprime' * margin_vec0;
  costs = clf.Cprime * margin_vec0;
  [min_pos_costs , ~] = min(costs(2:end,:));
  traces0(i,:) = -(min_pos_costs - costs(1,:));

  if use_trees
    ids1 = forestInds(X1,clf.thrs(:,i),clf.fids(:,i),clf.child(:,i), 1e5);
    for j=1:n1
      z = clf.hs(ids1(j),i);
      margin_vec1(:,j) = margin_vec1(:,j) + ...
                        (clf.wl_weights(i).*clf.Y(:, z));
    end
  else % general case with other weak learners.
    % z is a row vector with the labels
    z  = clf.classify_weak_learner(clf.WEAK_LEARNERS{i}, X1');
    for j=1:n1
      margin_vec1(:,j) = margin_vec1(:,j) + (clf.WEIGHTS(i).*clf.Y(:, z(j)));
    end
  end
  % WARNING: Change to accomodate with theory (2016/11)
%  costs = clf.Cprime' * margin_vec1;
  costs = clf.Cprime * margin_vec1;
  [min_pos_costs , ~] = min(costs(2:end,:));
  traces1(i,:) = -(min_pos_costs - costs(1,:));
end

% 2. plot the min and max trace at last weak learner of positives and
%    negatives data
[~ , min_neg_trace_index] = min(traces0(nWeaks,:));
[~ , max_neg_trace_index] = max(traces0(nWeaks,:));

[~ , min_pos_trace_index] = min(traces1(nWeaks,:));
[~ , max_pos_trace_index] = max(traces1(nWeaks,:));

h1 = figure;
plot(traces1(:,min_pos_trace_index), 'g-', 'LineWidth', 1.5);
hold on;
plot(traces1(:,max_pos_trace_index), 'g-', 'LineWidth', 1.5);
plot(traces0(:,min_neg_trace_index), 'r-', 'LineWidth', 1.5);
plot(traces0(:,max_neg_trace_index), 'r-', 'LineWidth', 1.5);

% 3. Compute the optimal threshold (for same success rate but with
% less average weak-learners evaluated per feature vector). For doing
% so, we find the trace of the last positive example that went over
% 0 at the last badacost weak-learner.
indices       = find(traces1(nWeaks,:) > 0);
[~, index] = min(traces1(nWeaks, indices));
index = indices(index);

figure(h1);
hold on;
plot(traces1(:,index), 'b-', 'LineWidth', 2);

min_last_traces1_pos = min(traces1(:,index));
thr = min_last_traces1_pos;

hold on;
plot([1 nWeaks], [min_last_traces1_pos min_last_traces1_pos], 'k-', 'LineWidth', 2);
plot([1 nWeaks], [thr thr], 'm-', 'LineWidth', 2);
hold off;


function minLoc = overlaysurface(fitnessfcn,options)
% Overlays surface and marks known minimum for demo cases of pso. Called by
% PSOPLOTSWARMSURF.
%
% See also: PSODEMO, PSOPLOTSWARMSURF

xBound = options.PopInitRange(:,1) ;
yBound = options.PopInitRange(:,2) ;
[XX,YY] = meshgrid(xBound(1):(xBound(2)-xBound(1))/50:xBound(2),...
    yBound(1):(yBound(2)-yBound(1))/50:yBound(2)) ;
ZZ = zeros(size(XX)) ;
clear xBound yBound

for i = 1:size(XX,1)
    for j = 1:size(XX,2)
        ZZ(i,j) = fitnessfcn([XX(i,j) YY(i,j)]) ;
    end
end

if strcmpi(options.DemoMode,'pretty') || strcmpi(options.DemoMode,'on')
    surface(XX,YY,ZZ,'LineStyle','none',...
        'FaceAlpha',0.4,...
        'FaceLighting','gouraud',...
        'FaceColor','interp')
    flagfill = [1 0 0] ;
    flaglines = [1 0 0] ;
    set(gcf,'Colormap',1-colormap('cool'))
else
    surface(XX,YY,ZZ,'LineStyle','-',...
        'FaceColor','none',...
        'EdgeColor',[1 0.5 0.5])
    flagfill = 'none' ;
    flaglines = [0 0.5 0] ;
%     set(gca,'Color','none')
%     set(gcf,'Colormap',colormap('summer'))
end
axis tight

% Mark known minimum location, if it exists
if ~isempty(options.KnownMin)
    xmin = options.KnownMin(1) ;
    ymin = options.KnownMin(2) ;
    zminmax = get(gca,'ZLim') ;
    minLoc = line([xmin xmin],[ymin ymin],zminmax,...
        'Color',flaglines,...
        'LineWidth',1) ;
    aspr = get(gca,'DataAspectRatio') ;
    flagtip = 0.125*diff(zminmax)*aspr(1)/aspr(3) ;
    patch([xmin xmin xmin + flagtip],...
        [ymin ymin ymin],...
        [zminmax(2),...
         zminmax(2) + 0.1*diff(zminmax),...
         zminmax(2) + 0.05*diff(zminmax)],[1 0 0],...
         'FaceColor',flagfill,...
         'LineWidth',1,...
         'EdgeColor',flaglines)
     clear xmin ymin
end

axis tight
camlight
set(gca,...
    'XLimMode','manual',...
    'YLimMode','manual',...
    'ZLimMode','manual',...
    'PlotBoxAspectRatio',[1 1 1])


function SO3VF = abs(F)
% overloads the componentwise absolute value |abs(SO3VF)|
%
% Note that abs is not the length of a vector3d which is the evaluation of
% the SO3VectorField in one rotation.
%
% one
%
% Syntax
%   SO3VF = abs(SO3VF)
%
% Input
%  SO3VF - @SO3VectorField
%
% Output
%  SO3VF - @SO3VectorField
%

SO3VF = SO3VectorFieldHandle(@(rot) g(rot),F.SRight,F.SLeft);

function g = g(rot)
g = F.eval(rot);
g = vector3d(abs(g.xyz.'));
end

end


function [rpav]=realized_preaveraged_bipower_variation(price,time,timeType,samplingType,samplingInterval,options)
% Estimated integrated variance using Preaveraged Bipower Variation
%
% USAGE:
%   [RPBV] = realized_preaveraged_bipower_variation(PRICE)
%   [RPBV] = realized_preaveraged_bipower_variation(PRICE,TIME,TIMETYPE,SAMPLINGTYPE,SAMPLINGINTERVAL)
%   [RPBV] = realized_preaveraged_bipower_variation(PRICE,TIME,TIMETYPE,SAMPLINGTYPE,SAMPLINGINTERVAL,OPTIONS)
%
% INPUTS:
%   PRICE            - m by 1 vector of high frequency prices
%   TIME             - [OPTIONAL] m by 1 vector of times where TIME(i) corresponds to PRICE(i).
%   TIMETYPE         - [OPTIONAL] String describing the way times are measured
%                       'wall'    24-hour clock of the form HHMMSS.mmm, e.g. 101543 or 153217
%                       'seconds' Time measured in seconds past midnight
%                       'unit'  Unit normalized date format, e.g. .1, .234, .9
%                         Unit normalized times are more general than the other types and can be
%                         applied to data from more than one calendar day
%   SAMPLINGTYPE     - [OPTIONAL] String describing the type of sampling to use when
%                        filtering PRICE
%                        'CalendarTime' - Sample in calendar time using observations separated by
%                          SAMPLINGINTERVAL seconds
%                        'CalendarUniform' - Sample in calendar time using SAMPLINGINTERVAL
%                          observations spread uniformly between TIME(1) and TIME(m)
%                        'BusinessTime' - Sample in business (tick) time using observation separated
%                          by SAMPLINGINTERVAL ticks
%                        'BusinessUniform' - Sample in business (tick) time using observations
%                          uniformly spaced in business time.
%                        'Fixed' - Sample at specific points in time. When using fixed,
%                          SAMPLINGINTERVAL must be a n by 1 vector of times with the same TIMETYPE
%                          as TIME (i.e. seconds if TIME is in seconds)
%   SAMPLINGINTERVAL  - [OPTIONAL] Scalar integer or n by 1 vector whose meaning depends on the
%                         selected SAMPLINGTYPE
%   OPTIONS           - [OPTIONAL] Preaveraged Realized Variance option structure initialized by calling
%                         realized_options('Preaveraging'). See help realized_options for a description of
%                         available options.
%
% OUTPUTS:
%   RPBV - Preaveraged bipower variation estimate
%
% COMMENTS:
%  Follows Christensen, Oomen and Podolski (2014) most closely, with the
%  noise variance estimator used in Hautsch and Podolski (2013)
%
% EXAMPLES:
%
%  See also PREALIZED_PREAVERAGED_VARIANCE, REALIZED_OPTIONS, REALIZED_KERNEL,
%  REALIZED_NOISE_ESTIMATE, REALIZED_VARIANCE, REALIZED_VARIANCE_OPTIMAL_SAMPLING,
%  REALIZED_RANGE, REALIZED_QUANTILE_VARIANCE

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 2/27/2014

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch nargin
    case 1
        m = length(price);
        time = linspace(9.5*3600,16*3600,m)';
        timeType = 'seconds';
        samplingType = 'businesstime';
        samplingInterval = 1;
        options = realized_options('preaveraging');
    case 5
        options = realized_options('preaveraging');
    case 6
        % Nothing
    otherwise
        error('One, five or six inputs required.')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Need method to estimate omega, and options.

% Inserted to protect against inputing integer times
time = double(time);

% 1. Filter the rpice
filteredPrice = realized_price_filter(price,time,timeType,samplingType,samplingInterval);
returns = diff(log(filteredPrice));
% 2. Compute rpreaveraged returns
theta = options.theta;
m = length(returns);
K = ceil(theta * sqrt(m));
g = @(x) min(x,1-x);
w = g((1:(K-1))/K);

preav_returns = nan(m-K+2,1);
for i=1:m-K+2
    preav_returns(i) = w*returns(i:i+K-2);
end

% 3. Compute constants
psi_1 = K*sum((g((1:K)/K) - g((0:(K-1))/K)).^2);
psi_2 = 1/K*sum((g((1:(K-1))/K)).^2);
% 4. Estimate noise
[noiseVariance, ~, ~, noiseEstimateOomen] = realized_noise_estimate(price, time, timeType, options);

omega = noiseEstimateOomen;
if omega<0
    omega = noiseVariance;
end

% 5. Compute Bipower Variation
mu_1 = sqrt(2/pi);
const1 = m/(m-2*K+2);
const2 = 1/(K*psi_2*mu_1^2);

bias = psi_1/(theta.^2 * psi_2) * omega^2;

lead = preav_returns(K:end);
lag = preav_returns(1:end-K+1);

rpav = const1 * const2 * sum(abs(lead.*lag)) - bias;