function [opts] = IPM_SC ( ipts , par)
% ========================================================================
% Spaese Coding of FDDL, Version 1.0
% Copyright(c) 2011  Meng YANG, Lei Zhang, Xiangchu Feng and David Zhang
% All Rights Reserved.
%
% -----------------------------------------------------------------------
% Permission to use, copy, or modify this software and its documentation
% for educational and research purposes only and without fee is here
% granted, provided that this copyright notice and the original authors'
% names appear on all copies and supporting documentation. This program
% shall not be used, rewritten, or adapted as the basis of a commercial
% software or hardware product without first obtaining permission of the
% authors. The authors make no representations about the suitability of
% this software for any purpose. It is provided "as is" without express
% or implied warranty.
%----------------------------------------------------------------------
%
% This is an implementation of the algorithm to do sparse coding
%
% Please refer to the following paper
%
% Meng Yang, Lei Zhang, Xiangchu Feng, and David Zhang,"Fisher Discrimination
% Dictionary Learning for Sparse Representation", In IEEE Int. Conf. on
% Computer Vision, 2011.
% L. Rosasco, A. Verri, M. Santoro, S. Mosci, and S. Villa. Iterative
% Projection Methods for Structured Sparsity Regularization. MIT Technical
% Reports, MIT-CSAIL-TR-2009-050,CBCL-282, 2009.
% J. Bioucas-Dias, M. Figueiredo, ?A new TwIST: two-step iterative shrinkage
% /thresholding  algorithms for image restoration?, IEEE Transactions on
% Image Processing, December 2007.
%----------------------------------------------------------------------
%
%  Inputs :   (1) ipts :    the structre of input data
%                    .D     the dictionary
%                    .tau1    parameter of l1-norm energy of coefficient
%                    .y     the testing sample
%             (2) par :     the struture of input parameters
%                    .eigenv
%
% Outputs:    (1) opts :    the structure of output data
%                    .A     the coefficient matrix
%                    .ert   the total energy sequence
%
%---------------------------------------------------------------------

ipts.cT         =      1e+10;  % stop criterion
ipts.citeT      =      1e-6;   % stop criterion
par.nIter       =      200;    % maximal iteration number
par.cRatio      =      1.05;
ipts.initM      =      'zero'; % coefficiet initialization method

switch lower(ipts.initM)
    case {'zero'}
        x(:,1)  =  zeros(size(ipts.D,2),1);
    case {'transpose'}
        x(:,1)  =  ipts.D'*ipts.y;
    case {'pinv'}
        x(:,1)  =  pinv(ipts.D)*ipts.y;
    otherwise
        error('Nonknown method!');
end

A         =         ipts.D;
tau1      =         ipts.tau1;
y         =         ipts.y;
nIter     =         par.nIter;
if ~isfield(par,{'eigenv'})
c         =         par.cRatio*find_max_eigenv(A'*A);
else
c         =         par.cRatio*par.eigenv ;
end

%%%%%%%%%%%%%%%%%%%%
% TWIST parameter
%%%%%%%%%%%%%%%%%%%%
for_ever           =         1;
IST_iters          =         0;
TwIST_iters        =         0;
sparse_sign        =         1;
verbose            =         1;
enforceMonotone    =         1;
lam1               =         1e-4;   %default eigenvalues
lamN               =         1;      %default eigenvalues
rho0               =         (1-lam1/lamN)/(1+lam1/lamN);
alpha              =         2/(1+sqrt(1-rho0^2));        %default,user can set
beta               =         alpha*2/(lam1+lamN);         %default,user can set

%%%%%%%%%%%%%%%%%%%
%main loop
%%%%%%%%%%%%%%%%%%%
xm2       =      x(:,1);
xm1       =      x(:,1);
prev_f    =      norm(y-A*x(:,1),2)^2+...
                    tau1*norm(x(:,1),1);
for n_it = 2 : nIter;

   ert(n_it-1)      =       norm(y-A*x(:,n_it-1),2)^2+...
        tau1*norm(x(:,n_it-1),1);
   fprintf('Iteration:%f  Total gap:%f\n',n_it,ert(n_it-1));

    while for_ever
       % IST(IPM) estimate
       v        =       1/c*A'*(y-A*xm1)+xm1;
       x_temp   =      soft(v,tau1/c/2);

       if (IST_iters >= 2) | ( TwIST_iters ~= 0)
            % set to zero the past when the present is zero
            % suitable for sparse inducing priors
            if sparse_sign
                mask    =   (x_temp ~= 0);
                xm1     =   xm1.* mask;
                xm2     =   xm2.* mask;
            end
            % two-step iteration
            xm2    =   (alpha-beta)*xm1 + (1-alpha)*xm2 + beta*x_temp;
            % compute residual
            f      =   norm(y-A*xm2,2)^2+...
                       tau1*norm(xm2,1);
            if (f > prev_f) & (enforceMonotone)
                TwIST_iters   =  0;  % do a IST(IPM) iteration if monotonocity fails
            else
                TwIST_iters =   TwIST_iters+1; % TwIST iterations
                IST_iters   =    0;
                x_temp      =   xm2;
                if mod(TwIST_iters,10000) ==0
                   c = 0.9*c;
                end
                break;  % break loop while
            end
        else
            f     =     norm(y-A*x_temp,2)^2+...
                        tau1*norm(x_temp,1);
            if f > prev_f
                % if monotonicity  fails here  is  because
                % max eig (A'A) > 1. Thus, we increase our guess
                % of max_svs
                c         =    2*c;
                if verbose
%                     fprintf('Incrementing c=%2.2e\n',c);
                end
                if  c > ipts.cT
                    break;  % break loop while
                end
                IST_iters = 0;
                TwIST_iters = 0;
            else
                TwIST_iters = TwIST_iters + 1;
                break;  % break loop while
            end
        end
    end

    citerion      =   abs(f-prev_f)/prev_f;
    if citerion < ipts.citeT | c > ipts.cT
%        fprintf('Stop!\n c=%2.2e\n citerion=%2.2e\n',c,citerion);
       break;
    end

    xm2           =   xm1;
    xm1           =   x_temp;
    x(:,n_it)   =   x_temp;
    prev_f        =   f;
end

opts.x     =       x(:,end);
opts.ert   =       ert;


%==============================================================================
% 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;
%==============================================================================


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


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 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=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.


% 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 [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;


%PoseGraph Pose graph

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

    properties
        graph

        ngrid
        center
        cellsize
    end

    methods

        function pg = PoseGraph(filename, varargin)
            % parse the file data
            % we assume g2o format
            %    VERTEX* vertex_id X Y THETA
            %    EDGE* startvertex_id endvertex_id X Y THETA IXX IXY IYY IXT IYT ITT
            % vertex numbers start at 0

            opt.laser = false;

            opt = tb_optparse(opt, varargin);

            pg.graph = PGraph(3, 'distance', 'SE2');

            fp = fopen(filename, 'r');
            assert(fp > 0, 'Can''t open file %s', filename);

            toroformat = false;
            nlaser = 0;

            % indices into ROBOTLASER1 record for the 3x3 info matrix in column major
            % order
            g2o =  [6  7  8  7  9  10   8  10  11];
            toro = [6  7 10  7  8  11  10  11   9];

            % we keep an array pgi = vindex(gi) to map g2o vertex index to PGraph vertex index

            tic
            while ~feof(fp)

                line = fgets(fp);

                % is it a comment?
                if line(1) == '#'
                    continue;
                end

                % get keyword
                k = strfind(line, ' ');

                % and deal with it
                switch line(1:k-1)
                    case 'VERTEX_SE2'
                        % g2o format vertex
                        vertex = sscanf(line(k+1:end), '%d %f %f %f')';
                        v = pg.graph.add_node(vertex(2:4));
                        vindex(vertex(1)+1) = v;
                        vd.type = 'vertex';
                        pg.graph.setvdata(v, vd);

                    case 'VERTEX_XY'
                        vertex = sscanf(line(k+1:end), '%d %f %f')';
                        v = pg.graph.add_node(vertex(2:4));
                        vindex(vertex(1)+1) = v;
                        vd.type = 'landmark';
                        pg.graph.setvdata(v, vd);

                    case 'EDGE_SE2'
                        % g2o format edge
                        edge = sscanf(line(k+1:end), '%f')';
                        v1 = vindex(edge(1)+1);
                        v2 = vindex(edge(2)+1);

                        % create the edge
                        e = pg.graph.add_edge(v1, v2);

                        % create the edge data as a structure
                        %  X  Y  T
                        %  3  4  5
                        ed.mean = edge(3:5);

                        % IXX IXY IXT IYY IYT ITT
                        %   6   7   8   9  10  11
                        ed.info = reshape(edge(g2o), [3 3]);

                        % and attach it
                        pg.graph.setedata(e, ed);

                    case 'VERTEX2'
                        toroformat = true;
                        vertex = sscanf(line(k+1:end), '%d %f %f %f')';
                        v = pg.graph.add_node(vertex(2:4));
                        vindex(vertex(1)+1) = v;
                        vd.type = 'vertex';
                        pg.graph.setvdata(v, vd);

                    case 'EDGE2'
                        toroformat = true;
                        edge = sscanf(line(k+1:end), '%f')';
                        v1 = vindex(edge(1)+1);
                        v2 = vindex(edge(2)+1);

                        % create the edge
                        e = pg.graph.add_edge(v1, v2);
                        % create the edge data as a structure
                        %  X  Y  T
                        %  3  4  5
                        ed.mean = edge(3:5);

                        % IXX IXY IYY ITT IXT IYT
                        %   6   7   8   9  10  11
                        ed.info = reshape(edge(toro), [3 3]);
                        % and attach it
                        pg.graph.setedata(e, ed);

                    case 'ROBOTLASER1'
                        if ~opt.laser
                            continue;
                        end

                        % laser records are associated with the immediately preceding VERTEX record
                        [laser,n] = sscanf(line(k+1:end), '%f');
                        nbeams = laser(8);
                        vd.theta = [0:nbeams-1] * laser(4) + laser(2);
                        vd.range = laser(9:8+nbeams)';
                        vd.time = laser(21+nbeams);
                        pg.graph.setvdata(v, vd);
                        nlaser = nlaser + 1;

                    otherwise
                        error('RTB:posegraph:badfile', 'Unexpected line  <%s> in %s', line(1:k-1), filename);
                end
            end
            elapsed = toc;

            fclose(fp);

            if toroformat
                fprintf('loaded TORO/LAGO format file: %d nodes, %d edges in %.2f sec\n', pg.graph.n, pg.graph.ne, elapsed);
            else
                fprintf('loaded g2o format file: %d nodes, %d edges in %.2f sec\n', pg.graph.n, pg.graph.ne, elapsed);
                if nlaser > 0
                    fprintf('  %d laser scans: %d beams, fov %g to %g deg, max range %g\n', ...
                        nlaser, nbeams, [laser(2) sum(laser(2:3))]*180/pi, laser(5) );
                end
            end
        end

        function [r, theta] = scan(pg, n)
            vd = pg.graph.vdata(n);
            r = vd.range;
            theta = vd.theta;
        end

        function [X,Y] = scanxy(pg, n)
            vd = pg.graph.vdata(n);

            [x,y] = pol2cart(vd.theta, vd.range);
            if nargout == 1
                X = [x; y];
            elseif nargout == 2
                X = x; Y = y;
            end
        end

        function plot_scan(pg, n)
            for i=n(:)'
                [x,y] = pg.scanxy(i);
                plot(x, y, '.', 'MarkerSize', 10);
                pause
            end
        end

        function xyt = pose(pg, i)
            xyt = pg.graph.coord(i);
        end

        function t = time(pg, n)
            t = pg.graph.vdata(n).time;
        end

        function plot(pg, varargin)
            pg.graph.plot(varargin{:});
            xlabel('x')
            ylabel('y')
            grid on
        end

        function world = scanmap(pg, varargin)

            opt.center = [75 50];
            opt.ngrid = 3000;
            opt.cellsize = 0.1;

            pg = tb_optparse(opt, varargin, pg);

            h = waitbar(0, 'rendering a map');

            world = zeros(pg.ngrid, pg.ngrid, 'int32');
            for i=1:1:pg.graph.n

                if rem(i, 20) == 0
                    waitbar(i/pg.graph.n, h)
                end

                xy = pg.scanxy(i);
                [r,theta] = pg.scan(i);
                xy(:,r>40) = [];
                xyt = pg.graph.coord(i);

                xy = SE2(xyt) * xy;

                % start of each ray
                [x1,y1] = pg.w2g(xyt(1:2));

                for s=1:numcols(xy)

                    % end of each ray
                    [x2,y2] = pg.w2g(xy(:,s));

                    % all cells along the ray
                    p = bresenham(x1, y1, x2, y2);
                    try
                        k = sub2ind(size(world), p(:,1), p(:,2));

                        k1 = k(1:end-1); k2 = k(end);
                        world(k1) = world(k1) - 1;
                        world(k2) = world(k2) + 1;
                    catch me
                        % come here if any point on the ray is outside the grid
                        % silently ignore it
                    end
                end
            end
            close(h)
            %idisp(world)
        end

        function [gx,gy] = w2g(pg, w)
            dd = 0.10;

            w = w(:) + pg.center(:);
            g = round(w/pg.cellsize);
            gx = g(1); gy = g(2);
        end

        function plot_occgrid(pg, w)

            x = [1:numcols(w)]*pg.cellsize - pg.center(1);
            y = [1:numrows(w)]*pg.cellsize - pg.center(2);
            w(w<0) = -1;
            w(w>0) = 1;
            w=-w;
            idisp(w, 'nogui', 'xydata', {x, y})
            xlabel('x'); ylabel('y');
        end

        %   This source code is part of the graph optimization package
        %   deveoped for the lectures of robotics2 at the University of Freiburg.
        %
        %     Copyright (c) 2007 Giorgio Grisetti, Gian Diego Tipaldi
        %
        %   It is licences under the Common Creative License,
        %   Attribution-NonCommercial-ShareAlike 3.0
        %
        %   You are free:
        %     - to Share - to copy, distribute and transmit the work
        %     - to Remix - to adapt the work
        %
        %   Under the following conditions:
        %
        %     - Attribution. You must attribute the work in the manner specified
        %       by the author or licensor (but not in any way that suggests that
        %       they endorse you or your use of the work).
        %
        %     - Noncommercial. You may not use this work for commercial purposes.
        %
        %     - Share Alike. If you alter, transform, or build upon this work,
        %       you may distribute the resulting work only under the same or
        %       similar license to this one.
        %
        %   Any of the above conditions can be waived if you get permission
        %   from the copyright holder.  Nothing in this license impairs or
        %   restricts the author's moral rights.
        %
        %   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.

        %ls-slam.m
        %this file is released under the creative common license

        %solves a graph-based slam problem via least squares
        %vmeans: matrix containing the column vectors of the poses of the vertices
        %	 the vertices are odrered such that vmeans[i] corresponds to the ith id
        %eids:	 matrix containing the column vectors [idFrom, idTo]' of the ids of the vertices
        %	 eids[k] corresponds to emeans[k] and einfs[k].
        %emeans: matrix containing the column vectors of the poses of the edges
        %einfs:  3d matrix containing the information matrices of the edges
        %	 einfs(:,:,k) refers to the information matrix of the k-th edge.
        %n:	 number of iterations
        %newmeans: matrix containing the column vectors of the updated vertices positions

        function g2 = optimize(pg, varargin)

            opt.iterations = 10;
            opt.animate = false;
            opt.retain = false;

            opt = tb_optparse(opt, varargin);

            g2 = PGraph(pg.graph);  % deep copy

            eprev = Inf;
            for i=1:opt.iterations
                if opt.animate
                    if ~opt.retain
                        clf
                    end
                    g2.plot();
                    pause(0.5)
                end

                [vmeans,energy] = linearize_and_solve(g2);
                g2.setcoord(vmeans);

                if energy >= eprev
                    break;
                end
                eprev = energy;
            end;

            pg.graph = g2;
        end
    end % methods
end % classdef

%computes the taylor expansion of the error function of the k_th edge
%vmeans: vertices positions
%eids:   edge ids
%emeans: edge means
%k:	 edge number
%e:	 e_k(x)
%A:	 d e_k(x) / d(x_i)
%B:	 d e_k(x) / d(x_j)
%function [e, A, B]=linear_factors(vmeans, eids, emeans, k)
function [e, A, B]=linear_factors(g, edge)
    %extract the ids of the vertices connected by the kth edge
    % 	id_i=eids(1,k);
    % 	id_j=eids(2,k);
    %extract the poses of the vertices and the mean of the edge
    %     v_i=vmeans(:,id_i);
    %     v_j=vmeans(:,id_j);
    %     z_ij=emeans(:,k);

    v = g.vertices(edge);
    v_i = g.coord(v(1));
    v_j = g.coord(v(2));
    z_ij = g.edata(edge).mean;

    %compute the homoeneous transforms of the previous solutions
    zt_ij=v2t(z_ij);
    vt_i=v2t(v_i);
    vt_j=v2t(v_j);

    %compute the displacement between x_i and x_j

    f_ij=(inv(vt_i)*vt_j);

    %this below is too long to explain, to understand it derive it by hand
    theta_i=v_i(3);
    ti=v_i(1:2,1);
    tj=v_j(1:2,1);
    dt_ij=tj-ti;

    si=sin(theta_i);
    ci=cos(theta_i);

    A= [-ci, -si, [-si, ci]*dt_ij; si, -ci, [-ci, -si]*dt_ij; 0, 0, -1 ];
    B =[  ci, si, 0           ; -si, ci, 0            ; 0, 0, 1 ];

    ztinv=inv(zt_ij);
    e=t2v(ztinv*f_ij);
    ztinv(1:2,3) = 0;
    A=ztinv*A;
    B=ztinv*B;

    % 	%compute the homogeneous transforms of the previous solutions
    % 	zt_ij=v2t(z_ij);
    % 	vt_i=v2t(v_i);
    % 	vt_j=v2t(v_j);
    % %     zt_ij = SE2(z_ij);
    % % 	vt_i = SE2(v_i);
    % % 	vt_j = SE2(v_j);
    %
    % 	%compute the displacement between x_i and x_j
    % 	%f_ij=(inverse(vt_i)*vt_j);
    % 	f_ij = vt_i.inv * vt_j;
    %
    % 	%this below is too long to explain, to understand it derive it by hand
    %       	theta_i=v_i(3);
    % 	ti=v_i(1:2);
    % 	tj=v_j(1:2);
    %       	dt_ij=tj-ti;
    %
    % 	si=sin(theta_i);
    % 	ci=cos(theta_i);
    %
    % 	A= [-ci, -si, [-si, ci]*dt_ij; si, -ci, [-ci, -si]*dt_ij; 0, 0, -1 ];
    % 	B =[  ci, si, 0           ; -si, ci, 0            ; 0, 0, 1 ];
    %
    % 	ztinv = inv(zt_ij);
    % 	e = xyt(ztinv*f_ij);
    % 	ztinv.t = 0;
    % 	A = ztinv*A;
    % 	B = ztinv*B;
end

%linearizes and solves one time the ls-slam problem specified by the input
%vmeans:   vertices positions at the linearization point
%eids:     edge ids
%emeans:   edge means
%einfs:    edge information matrices
%newmeans: new solution computed from the initial guess in vmeans
function [newmeans,energy] = linearize_and_solve(g)
    tic
    fprintf('solving');

    % H and b are respectively the system matrix and the system vector
    H=zeros(g.n*3,g.n*3);
    b=zeros(g.n*3,1);
    % this loop constructs the global system by accumulating in H and b the contributions
    % of all edges (see lecture)
    %for k=1:size(eids,2)
    fprintf('.');

    etotal = 0;
    for edge = 1:g.ne

        [e, A, B]=linear_factors(g, edge);
        omega = g.edata(edge).info;
        %compute the blocks of H^k

        % not quite sure whey SE3 is being transposed, what does that mean?
        b_i = -A'*omega*e;
        b_j = -B'*omega*e;
        H_ii = A'*omega*A;
        H_ij = A'*omega*B;
        H_jj = B'*omega*B;

        v = g.vertices(edge);
        id_i = v(1); id_j = v(2);
        %accumulate the blocks in H and b
        H((id_i-1)*3+1:id_i*3,(id_i-1)*3+1:id_i*3) = H((id_i-1)*3+1:id_i*3,(id_i-1)*3+1:id_i*3) + H_ii;
        H((id_j-1)*3+1:id_j*3,(id_j-1)*3+1:id_j*3) = H((id_j-1)*3+1:id_j*3,(id_j-1)*3+1:id_j*3) + H_jj;
        H((id_i-1)*3+1:id_i*3,(id_j-1)*3+1:id_j*3) = H((id_i-1)*3+1:id_i*3,(id_j-1)*3+1:id_j*3) + H_ij;
        H((id_j-1)*3+1:id_j*3,(id_i-1)*3+1:id_i*3) = H((id_j-1)*3+1:id_j*3,(id_i-1)*3+1:id_i*3) + H_ij';

        b((id_i-1)*3+1:id_i*3,1) = b((id_i-1)*3+1:id_i*3,1) + b_i;
        b((id_j-1)*3+1:id_j*3,1) = b((id_j-1)*3+1:id_j*3,1) + b_j;

        %NOTE on Matlab compatibility: note that we use the += operator which is octave specific
        %using H=H+.... results in a tremendous overhead since the matrix would be entirely copied every time
        %and the matrix is huge
        etotal = etotal + e'*e;
    end;
    fprintf('.');

    %note that the system (H b) is obtained only from
    %relative constraints. H is not full rank.
    %we solve the problem by anchoring the position of
    %the the first vertex.
    %this can be expressed by adding the equation
    %  deltax(1:3,1)=0;
    %which is equivalent to the following
    H(1:3,1:3) = H(1:3,1:3) + eye(3);

    SH=sparse(H);
    fprintf('.');
    deltax=SH\b;
    fprintf('.');

    %split the increments in nice 3x1 vectors and sum them up to the original matrix
    newmeans = g.coord()+reshape(deltax,3,g.n);

    %normalize the angles between -PI and PI
    for (i=1:size(newmeans,2))
        s=sin(newmeans(3,i));
        c=cos(newmeans(3,i));
        newmeans(3,i)=atan2(s,c);
    end
    dt = toc;
    fprintf('done in %.2g sec.  Total cost %g \n', dt, etotal);
    if nargout > 1
        energy = etotal;
    end
end

%   This source code is part of the graph optimization package
%   deveoped for the lectures of robotics2 at the University of Freiburg.
%
%     Copyright (c) 2007 Giorgio Grisetti, Gian Diego Tipaldi
%
%   It is licences under the Common Creative License,
%   Attribution-NonCommercial-ShareAlike 3.0
%
%   You are free:
%     - to Share - to copy, distribute and transmit the work
%     - to Remix - to adapt the work
%
%   Under the following conditions:
%
%     - Attribution. You must attribute the work in the manner specified
%       by the author or licensor (but not in any way that suggests that
%       they endorse you or your use of the work).
%
%     - Noncommercial. You may not use this work for commercial purposes.
%
%     - Share Alike. If you alter, transform, or build upon this work,
%       you may distribute the resulting work only under the same or
%       similar license to this one.
%
%   Any of the above conditions can be waived if you get permission
%   from the copyright holder.  Nothing in this license impairs or
%   restricts the author's moral rights.
%
%   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.

%computes the homogeneous transform matrix A of the pose vector v
function A=v2t(v)
    c=cos(v(3));
    s=sin(v(3));
    A=[c, -s, v(1) ;
        s,  c, v(2) ;
        0   0  1  ];
end

%computes the pose vector v from an homogeneous transform A
function v=t2v(A)
    v(1:2, 1)=A(1:2,3);
    v(3,1)=atan2(A(2,1),A(1,1));
end


function [faces,idx,facemap]=uniqfaces(elem)
%
% [faces,idx,facemap]=uniqfaces(elem)
%
% return the unique face list from a or tetrahedral mesh
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%     elem: a list of elements, each row is a list of nodes for an element.
%           elem can have 2, 3 or 4 columns
%
% output:
%     face: unique faces in the mesh, denoted by a triplet of node indices
%     idx:  index of the output in the raw face list (returned by meshface)
%     facemap: index of the raw faces in the output list (for triangular mesh)
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

if(size(elem)==3)
   faces=elem;
elseif(size(elem)>=4)
   faces=meshface(elem);
else
   error('invalid input');
end

[ufaces,idx,jdx]=unique(sort(faces,2),'rows');
faces=faces(idx,:);
if(nargout>2)
    facemap=reshape(jdx,[size(elem,1) nchoosek(size(elem,2),3)]);
end


function t = subsasgn(t,s,rhs)
%SUBSASGN Subscripted assignment for sparse tensor.
%
%   We can assign elements to a sptensor in three ways.
%
%   Case 1: X(R1,R2,...,RN) = Y, in which case we replace the
%   rectangular subtensor (or single element) specified by the ranges
%   R1,...,RN with Y. The right-hand-side can be a scalar or an
%   sptensor.
%
%   Case 2: X(S) = V, where S is a p x n array of subscripts and V is
%   a scalar value or a vector containing p values.
%
%   Linear indexing is not supported for sparse tensors.
%
%   Examples
%   X = sptensor([30 40 20]) %<-- Create an emtpy 30 x 40 x 20 sptensor
%   X(30,40,20) = 7 %<-- Assign a single element to be 7
%   X([1,1,1;2,2,2]) = 1 %<-- Assign a list of elements to the same value
%   X(11:20,11:20,11:20) = sptenrand([10,10,10],10) %<-- subtensor!
%   X(31,41,21) = 7 %<-- grows the size of the tensor
%   X(111:120,111:120,111:120) = sptenrand([10,10,10],10) %<-- grows
%   X(1,1,1,1) = 4 %<-- increases the number of dimensions from 3 to 4
%
%   X = sptensor([30]) %<-- empty one-dimensional tensor
%   X([4:6]) = 1 %<-- set subtensor to ones (does not increase dimension)
%   X([10;12;14]) = (4:6)'  %<-- set three elements
%   X(31) = 7 %<-- grow the first dimension
%   X(1,1) = 0 %<-- add a dimension, but no nonzeros
%
%   Note regarding singleton dimensions: It is not possible to do, for
%   instance, X(1,1:10,1:10) = sptenrand([1 10 10],5). However, it is okay
%   to do X(1,1:10,1:10) = squeeze(sptenrand([1 10 10],5)).
%
%   See also SPTENSOR, TENSOR/SUBSASGN.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

switch s.type

    case '.'
        error(['Cannot change field ', s.subs, ' directly.']);

    case '()'

        % Do nothing if both subscripts and RHS are empty
        if isempty(s.subs{1}) && isempty(t.vals)
            return;
        end

        % Figure out if we are doing a subtensor or a list of subscripts...
        type = tt_assignment_type(t,s.subs,rhs);

        %% Case I: Replace a sub-tensor
        if isequal(type,'subtensor')

            %% Case I(a): RHS is another sparse tensor
            if isa(rhs,'sptensor')

                %% First, Resize the tensor and check the size match with
                %% the tensor that's being inserted.
                m = 1;
                for n = 1:numel(s.subs)
                    if ischar(s.subs{n}) && (s.subs{n} == ':')
                        if ndims(t) < n
                            newsz(1,n) = rhs.size(m);
                        else
                            newsz(1,n) = max([t.size(n), rhs.size(m)]);
                        end
                        m = m + 1;
                    elseif numel(s.subs{n}) == 1
                        if ndims(t) < n
                            newsz(1,n) = s.subs{n};
                        else
                            newsz(1,n) = max([t.size(n) s.subs{n}]);
                        end
                    else
                        if numel(s.subs{n}) ~= rhs.size(m)
                            error('RHS does not match range size');
                        end
                        if ndims(t) < n
                            newsz(1,n) = max(s.subs{n});
                        else
                            newsz(1,n) = max([t.size(n) s.subs{n}]);
                        end
                        m = m + 1;
                    end
                end
                t.size = newsz;

                % Expand subs array if there are new modes, i.e., if the order
                % has increased.
                if ~isempty(t.subs) && (size(t.size,2) > size(t.subs,2))
                    t.subs(:,end+1:size(t.size,2)) = 1;
                end

                % Delete what currently occupies the specified range
                rmloc = subdims(s.subs,t);
                kploc = setdiff(1:nnz(t),rmloc);
                newsubs = t.subs(kploc,:);
                newvals = t.vals(kploc);

                % Renumber the subscripts
                addsubs = irenumber(rhs, t.size, s.subs);
                t.subs = [newsubs; addsubs];
                t.vals = [newvals; rhs.vals];

                return;
            end

            %% Case I(b): RHS is zero or scalar

            % First, Resize the tensor.
            % Determine new size of existing modes
            for n = 1:ndims(t)
                if ischar(s.subs{n}) && (s.subs{n} == ':')
                    newsz(1,n) = t.size(n);
                else
                    newsz(1,n) = max([t.size(n) s.subs{n}]);
                end
            end
            % Determine size of new modes, if any
            for n = ndims(t)+1:numel(s.subs)
                newsz(1,n) = max(s.subs{n});
            end
            t.size = newsz;

            % Expand subs array if there are new modes, i.e., if the order
            % has increased.
            if ~isempty(t.subs) && (size(t.size,2) > size(t.subs,2))
                t.subs(:,end+1:size(t.size,2)) = 1;
            end

            % Case I(b)i: Zero right-hand-side
            if numel(rhs) == 1 && rhs == 0

                % Delete what currently occupies the specified range
                rmloc = subdims(s.subs,t);
                kploc = setdiff(1:nnz(t),rmloc);
                t.subs = t.subs(kploc,:);
                t.vals = t.vals(kploc);
                return;
            end

            % Case I(b)ii: Scalar right-hand-side
            if numel(rhs) == 1

                % Determine number of dimensions (may be larger than
                % current number)
                N = numel(s.subs);

                % Figure out how many indices are in each dimension
                nssubs = zeros(N,1);
                for n = 1:N
                    if ischar(s.subs{n}) && s.subs{n} == ':'
                        s.subs{n} = 1:size(t,n);
                    end
                    nssubs(n) = numel(s.subs{n});
                end

                % Preallocate (discover any memory issues here!)
                addsubs = zeros(prod(nssubs),N);

                % Generate appropriately sized ones vectors.
                o = cell(N,1);
                for n = 1:N
                    o{n} = ones(nssubs(n),1);
                end

                % Generate each column of the subscripts in turn
                for n = 1:N
                    i = o;
                    i{n} = s.subs{n}';
                    addsubs(:,n) = khatrirao(i);
                end

                if ~isempty(t.subs)
                    % replace existing values
                    [junk,loc] = intersect(t.subs,addsubs,'rows');
                    t.vals(loc) = rhs;
                    % pare down list of subscripts to add
                    addsubs = setdiff(addsubs,t.subs,'rows');
                end
                t.subs = [t.subs; addsubs];
                t.vals = [t.vals; rhs*ones(size(addsubs,1),1)];
                return;
            end

            error('Invalid RHS')

        end

        % Case II: Subscripts
        if isequal(type,'subscripts')

            % Case II: Replacing values at specified indices

            newsubs = [s.subs{1}];
            tt_subscheck(newsubs);

            % Error check on subscripts
            if size(newsubs,2) < ndims(t)
                error('Invalid subscripts');
            end

            % Check for expanding the order
            if size(newsubs,2) > ndims(t)
                t.size(end+1:size(newsubs,2)) = 1;
                if ~isempty(t.subs)
                    t.subs(:,end+1:size(newsubs,2)) = 1;
                end
            end

            % Copy rhs to newvals
            newvals = rhs;

            % Error check the RHS is a column vector. We do not bother to
            % handle any other type of RHS with the sparse tensor.
            tt_valscheck(newvals);

            % Determine number of nonzeros being inserted. (This is
            % determined by the number of subscripts. Later we will check
            % to see that it matches the size of the RHS.)
            newnnz = size(newsubs,1);

            % Error check on size of newvals
            if numel(newvals) == 1

                % Special case where newvals is a single element to be
                % assigned to multiple RHS. Fix to be correct size.
                newvals = newvals * ones(newnnz,1);

            elseif size(newvals,1) ~= newnnz

                % Sizes don't match!
                error('Number of subscripts and number of values do not match!');

            end

            % Remove duplicates & print warning if any duplicates were
            % removed.
            [newsubs,idx] = unique(newsubs,'rows');
            if size(newsubs,1) ~= newnnz
                warning('Duplicate assignments discarded.');
            end
            newvals = newvals(idx);

            % Find which subscripts already exist and their locations
            [tf,loc] = ismember(newsubs,t.subs,'rows');

            % Split into three groups for processing:
            %
            % Group A: Elements that already exist and need to be changed
            % Group B: Elements that already exist and need to be removed
            % Group C: Elements that do not exist and need to be added
            %
            % Note that we are ignoring any new zero elements, because
            % those obviously do not need to be added. Also, it's
            % important to process Group A before Group B because the
            % processing of Group B may change the locations of the
            % remaining elements.

            idxa = find((tf .* newvals) ~= 0);
            idxb = find((tf .* ~abs(newvals)) ~= 0);
            idxc = find((~tf .* newvals) ~= 0);

            % Process Group A: Changing values
            if ~isempty(idxa)
                t.vals(loc(idxa)) = newvals(idxa);
            end

            % Process Group B: Removing values
            if ~isempty(idxb)
                removesubs = loc(idxb);
                keepsubs = setdiff(1:nnz(t),removesubs);
                t.subs = t.subs(keepsubs,:);
                t.vals = t.vals(keepsubs);
            end

            % Process Group C: Adding new, nonzero values
            if ~isempty(idxc)
                t.subs = [t.subs; newsubs(idxc,:)];
                t.vals = [t.vals; newvals(idxc)];
            end

            % Resize the tensor!
            for n = 1:length(t.size)
                smax = max(newsubs(:,n));
                t.size(n) = max(t.size(n), smax);
            end

            return;

        end

        error('Invalid call to sptensor/subsasgn');

    case '{}'
        error('Subscript cell reference not supported for sptensor.');

    otherwise
        error('Incorrect indexing into sptensor.')

end