function [] = plot_way(ax, parsed_osm, map_img_filename)
%PLOT_WAY   plot parsed OpenStreetMap file
%
% usage
%   PLOT_WAY(ax, parsed_osm)
%
% input
%   ax = axes object handle
%   parsed_osm = parsed OpenStreetMap (.osm) XML file,
%                as returned by function parse_openstreetmap
%   map_img_filename = map image filename to load and plot under the
%                      transportation network
%                    = string (optional)
%
% 2010.11.06 (c) Ioannis Filippidis, jfilippidis@gmail.com
%
% See also PARSE_OPENSTREETMAP, EXTRACT_CONNECTIVITY.

% ToDo
%   add double way roads

if nargin < 3
    map_img_filename = [];
end

[bounds, node, way, ~] = assign_from_parsed(parsed_osm);

disp_info(bounds, size(node.id, 2), size(way.id, 2))
show_ways(ax, bounds, node, way, map_img_filename);

function [] = show_ways(hax, bounds, node, way, map_img_filename)
show_map(hax, bounds, map_img_filename)

%plot(node.xy(1,:), node.xy(2,:), '.')

key_catalog = {};
for i=1:size(way.id, 2)
    [key, val] = get_way_tag_key(way.tag{1,i} );

    % find unique way types
    if isempty(key)
        %
    elseif isempty( find(ismember(key_catalog, key) == 1, 1) )
        key_catalog(1, end+1) = {key};
    end

    % way = highway or amenity ?
    flag = 0;
    switch key
        case 'highway'
            flag = 1;

            % bus stop ?
            if strcmp(val, 'bus_stop')
                disp('Bus stop found')
            end
        case 'amenity'
            % bus station ?
            if strcmp(val, 'bus_station')
                disp('Bus station found')
            end
        otherwise
            disp('way without tag.')
    end

    % plot highway
    way_nd_ids = way.nd{1, i};
    num_nd = size(way_nd_ids, 2);
    nd_coor = zeros(2, num_nd);
    nd_ids = node.id;
    for j=1:num_nd
        cur_nd_id = way_nd_ids(1, j);
        if ~isempty(node.xy(:, cur_nd_id == nd_ids))
             nd_coor(:, j) = node.xy(:, cur_nd_id == nd_ids);
        end
    end

    % remove zeros
    nd_coor(any(nd_coor==0,2),:)=[];

    if ~isempty(nd_coor)
        % plot way (highway = blue, other = green)
        if flag == 1
            plot(hax, nd_coor(1,:), nd_coor(2,:), 'b-')
        else
            plot(hax, nd_coor(1,:), nd_coor(2,:), 'g--')
        end
    end

    %waitforbuttonpress
end
disp(key_catalog.')

function [] = disp_info(bounds, Nnode, Nway)
disp( ['Bounds: xmin = ' num2str(bounds(1,1)),...
    ', xmax = ', num2str(bounds(1,2)),...
    ', ymin = ', num2str(bounds(2,1)),...
    ', ymax = ', num2str(bounds(2,2)) ] )
disp( ['Number of nodes: ' num2str(Nnode)] )
disp( ['Number of ways: ' num2str(Nway)] )


function dtiRawEddyCorrect(dwRaw, mnB0, outEddyCorrectXform)
%
% dtiRawEddyCorrect([dwRaw=uigetfile], [mnB0=uigetfile], [outEddyCorrectXform])
%
% Aligns each raw DW image in dwRaw (NIFTI format) to the mean
% b=0 image, saving all the resulting xforms in outEddyCorrectXform.
%
% HISTORY:
% 2007.04.23 RFD: wrote it.

% Initialize SPM default params
spm_defaults; global defaults;
estParams = defaults.coreg.estimate;

%% Load the raw DW data (in NIFTI format)
if(~exist('dwRaw','var')|isempty(dwRaw))
    [f,p] = uigetfile({'*.nii.gz;*.nii';'*.*'}, 'Select the raw DW NIFTI dataset...');
    if(isnumeric(f)) error('User cancelled.'); end
    dwRaw = fullfile(p,f);
end
if(ischar(dwRaw))
    % dwRaw can be a path to the file or the file itself
    [dataDir,inBaseName] = fileparts(dwRaw);
else
    [dataDir,inBaseName] = fileparts(dwRaw.fname);
end
[junk,inBaseName,junk] = fileparts(inBaseName);
if(isempty(dataDir)) dataDir = pwd; end

% Load the b0 data (in NIFTI format)
if(~exist('mnB0','var')|isempty(mnB0))
   mnB0 = fullfile(dataDir, [inBaseName '_b0.nii.gz']);
   [f,p] = uigetfile({'*.nii.gz;*.nii';'*.*'}, 'Select the mean b0 NIFTI dataset...',mnB0);
   if(isnumeric(f)) error('User cancelled.'); end
   mnB0 = fullfile(p,f);
end
if(ischar(mnB0))
    disp(['Loading b0 data ' mnB0 '...']);
    mnB0 = niftiRead(mnB0);
end

if(~exist('outEddyCorrectXform','var')|isempty(outEddyCorrectXform))
    outEddyCorrectXform = fullfile(dataDir,[inBaseName 'EcXform']);
end

if(ischar(dwRaw))
    disp(['Loading raw data ' dwRaw '...']);
    dwRaw = niftiRead(dwRaw);
end

nvols = size(dwRaw.data,4);
dtMm = dwRaw.pixdim(1:3);

%% Eddy correction (will also correct for motion)
% Compute an affine xform to align each DW image to the mean B0
% image.
% Affine (12-params)
estParams.params = [0 0 0 0 0 0 1 1 1 0 0 0];
estParams.sep = [2];
target.uint8 = uint8(round(mrAnatHistogramClip(double(mnB0.data),0.4,0.99)*255));
target.mat = [diag(dtMm), [-[size(target.uint8)/2].*dtMm]'; 0 0 0 1];
source.mat = target.mat;
for(ii=1:nvols)
    fprintf('Aligning vol %d of %d to mean b=0 image', ii, nvols);
    if(ii>2)
        % Skip the first since it's usually the reference.
        estRemain = mean(et(2:end))*(nvols-ii+1)./60;
        if(estRemain>90) estRemain = estRemain./60; estUnits = 'hours';
        else estUnits = 'minutes'; end
        fprintf(' (previous iteration took %0.1f secs; %0.1f %s remaining)...\n',et(ii-1),estRemain,estUnits);
    else
        fprintf('\n');
    end
    tic;
    im = double(dwRaw.data(:,:,:,ii));
    source.uint8 = uint8(round(mrAnatHistogramClip(im,0.4,0.99)*255));
    msg = evalc('xformParams = spm_coreg(source,target,estParams);');
    xform{ii} = inv(target.mat)*spm_matrix(xformParams)*source.mat;
    et(ii) = toc;
end
disp(['Saving eddy/motion correction transforms to ' outEddyCorrectXform '...']);
disp('These transforms map voxels in each of the volumes to the reference image (usually the mean b=0).');
save(outEddyCorrectXform, 'xform');
fn = [outEddyCorrectXform '.txt'];
dlmwrite(fn,xform{1},'delimiter',' ','precision',6);
for(ii=2:length(xform))
    dlmwrite(fn,xform{ii},'delimiter',' ','roffset',1,'-append','precision',6);
end

return;


function [mri] = align_ctf2acpc(mri, opt, template)

% ALIGN_CTF2ACPC performs an approximate rigid body alignment of the anatomical
% volume from CTF towards ACPC coordinates. Only the homogeneous transformation
% matrix is modified and the coordsys-field is updated.
%
% Use as
%   mri = align_ctf2acpc(mri)
%   mri = align_ctf2acpc(mri, opt)
%   mri = align_ctf2acpc(mri, opt, template)
%
% The first input argument is a FieldTrip MRI-structure, and the second optional
% argument specifies how the registration is to be done:
%   method = 0: only an approximate coregistration
%   method = 1: an approximate coregistration, followed by spm_affreg
%   method = 2: an approximate coregistration, followed by spm_normalise (default)
%
% When opt = 1 or 2, an optional template filename can be specified, which denotes
% the filename of the target volume. This is required when running in deployed
% mode.
%
% See also ALIGN_NEUROMAG2ACPC, ALIGN_FSAVERAGE2MNI

if nargin<2
  opt = 2;
end

%--------------------------------------------------------------------------
% do a first round of approximate coregistration using the predefined voxel
% locations of the fiducials and landmarks in the template T1 image from
% SPM

acpcvox2acpchead = [
  2     0     0   -92
  0     2     0  -128
  0     0     2   -74
  0     0     0     1
  ];

% these are the voxel indices of some points in the SPM canonical T1
acpcvox_Ac           = [46 64  37  1]';     % the anterior commissure
acpcvox_Ori          = [46 48  10  1]';     % approximately between the ears in T1.mnc
acpcvox_Nas          = [46 106 13  1]';     % approximately the nasion in T1.mnc
acpcvox_Lpa_canal    = [ 5 48  10  1]';     % Left ear canal
acpcvox_Rpa_canal    = [87 48  10  1]';     % Right ear canal

acpchead_Ac           = acpcvox2acpchead * acpcvox_Ac  ;
acpchead_Ori          = acpcvox2acpchead * acpcvox_Ori ;
acpchead_Nas          = acpcvox2acpchead * acpcvox_Nas ;
acpchead_Lpa_canal    = acpcvox2acpchead * acpcvox_Lpa_canal ;
acpchead_Rpa_canal    = acpcvox2acpchead * acpcvox_Rpa_canal ;

ctfvox2ctfhead   = mri.transform;
acpchead2ctfhead = ft_headcoordinates(acpchead_Nas(1:3), acpchead_Lpa_canal(1:3), acpchead_Rpa_canal(1:3), 'ctf');

%ctfvox2acpchead =  inv(acpchead2ctfhead) *  ctfvox2ctfhead;
ctfvox2acpchead =  acpchead2ctfhead \ ctfvox2ctfhead;

% change the transformation matrix, such that it returns approximate SPM head coordinates
mri.transform     = ctfvox2acpchead;
mri.vox2headOrig  = ctfvox2ctfhead;
mri.vox2head      = ctfvox2acpchead;
mri.head2headOrig = acpchead2ctfhead;
mri.coordsys      = 'acpc';

%--------------------------------------------------------------------------
% Do a second round of affine registration (rigid body) to get improved
% alignment with ACPC coordinate system. this is needed because there may be
% different conventions defining LPA and RPA. The affine registration may
% fail however, e.g. if the initial alignment is not close enough. In that
% case SPM will throw an error

if opt==1
  % use spm_affreg

  switch lower(spm('ver'))
    case 'spm2'
      if isdeployed
        if nargin<3, ft_error('you need to specify a template filename when in deployed mode and using opt==1'); end
      else
        template = fullfile(spm('Dir'),'templates','T1.mnc');
      end

    case 'spm8'
      if isdeployed
        if nargin<3, ft_error('you need to specify a template filename when in deployed mode and using opt==1'); end
      else
        template = fullfile(spm('Dir'),'templates','T1.nii');
      end

    case 'spm12'
      if isdeployed
        if nargin<3, ft_error('you need to specify a template filename when in deployed mode and using opt==1'); end
      else
        template = fullfile(spm('Dir'),'toolbox','OldNorm','T1.nii');
        if ~exist('spm_affreg', 'file')
          addpath(fullfile(spm('Dir'),'toolbox','OldNorm'));
        end
      end
      fprintf('using ''OldNorm'' affine registration\n');

    otherwise
      ft_error('unsupported SPM version');
  end
  mri2 = ft_read_mri(template);

  tname1 = [tempname, '.img'];
  tname2 = [tempname, '.img'];
  V1 = ft_write_mri(tname1, mri.anatomy,  'transform', mri.transform,  'spmversion', spm('ver'), 'dataformat', 'nifti_spm');
  V2 = ft_write_mri(tname2, mri2.anatomy, 'transform', mri2.transform, 'spmversion', spm('ver'), 'dataformat', 'nifti_spm');

  % the below, using just spm_affreg does not work robustly enough in some cases
  flags.regtype = 'rigid';
  [M, scale]    = spm_affreg(V1,V2,flags);

  % some juggling around with the transformation matrices
  ctfvox2acpchead2  = M \ V1.mat;
  acpchead2ctfhead2 = ctfvox2ctfhead / ctfvox2acpchead2;

  % update the transformation matrix
  mri.transform     = ctfvox2acpchead2;

  % this one is unchanged
  mri.vox2headOrig  = ctfvox2ctfhead;

  % these are new
  mri.vox2head      = ctfvox2acpchead2;
  mri.head2headOrig = acpchead2ctfhead2;

  % delete the temporary files
  delete(tname1); delete(strrep(tname1, 'img', 'hdr'));
  delete(tname2); delete(strrep(tname2, 'img', 'hdr'));

elseif opt==2
  % use spm_normalise

  switch lower(spm('ver'))
    case 'spm2'
      if isdeployed
        if nargin<3, ft_error('you need to specify a template filename when in deployed mode and using opt==2'); end
      else
        template = fullfile(spm('Dir'),'templates','T1.mnc');
      end

    case 'spm8'
      if isdeployed
        if nargin<3, ft_error('you need to specify a template filename when in deployed mode and using opt==2'); end
      else
        template = fullfile(spm('Dir'),'templates','T1.nii');
      end

    case 'spm12'
      % this uses the 'OldNorm' functionality, so the path needs to be
      % added, can only be done if non-deployed.
      if isdeployed
        if nargin<3, ft_error('you need to specify a template filename when in deployed mode and using opt==2'); end
      else
        template = fullfile(spm('Dir'),'toolbox','OldNorm','T1.nii');
        if ~exist('spm_normalise', 'file')
          addpath(fullfile(spm('Dir'),'toolbox','OldNorm'));
        end
      end
      fprintf('using ''OldNorm'' normalisation\n');

    otherwise
      ft_error('unsupported SPM version');
  end
  mri2 = ft_read_mri(template);

  tname1 = [tempname, '.img'];
  tname2 = [tempname, '.img'];
  V1 = ft_write_mri(tname1, mri.anatomy,  'transform', mri.transform,  'spmversion', spm('ver'), 'dataformat', 'nifti_spm');
  V2 = ft_write_mri(tname2, mri2.anatomy, 'transform', mri2.transform, 'spmversion', spm('ver'), 'dataformat', 'nifti_spm');

  flags.nits       = 0; %set number of non-linear iterations to zero
  flags.regtype    = 'rigid';
  params           = spm_normalise(V2,V1,[],[],[],flags);
  acpchead2ctfhead2 = acpchead2ctfhead*V1.mat*params.Affine/V2.mat;
  ctfvox2acpchead2  = acpchead2ctfhead2\ctfvox2ctfhead;

  % update the transformation matrix
  mri.transform     = ctfvox2acpchead2;

  % this one is unchanged
  mri.vox2headOrig  = ctfvox2ctfhead;

  % these are new
  mri.vox2head      = ctfvox2acpchead2;
  mri.head2headOrig = acpchead2ctfhead2;

  % delete the temporary files
  delete(tname1); delete(strrep(tname1, 'img', 'hdr'));
  delete(tname2); delete(strrep(tname2, 'img', 'hdr'));
end


% Copyright (C) 2017-2018 Titus Cieslewski, RPG, University of Zurich,
%   Switzerland
%   You can contact the author at <titus at ifi dot uzh dot ch>
% Copyright (C) 2017-2018 Siddharth Choudhary, College of Computing,
%   Georgia Institute of Technology, Atlanta, GA, USA
% Copyright (C) 2017-2018 Davide Scaramuzza, RPG, University of Zurich,
%   Switzerland
%
% This file is part of dslam_open.
%
% dslam_open is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% dslam_open is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with dslam_open. If not, see <http://www.gnu.org/licenses/>.

function plotAccuracy(accuracy_measurements, num_robots, vo_end_time, ...
    orb_ate)

times = cellfun(@(x) x.time, accuracy_measurements);
num_meas = numel(accuracy_measurements);

robot_accuracies = cell(num_robots, 1);
mg_sizes = cell(num_robots, 1);
og_sizes = cell(num_robots, 1);
for robot_i = 1:num_robots
    robot_accuracies{robot_i} = zeros(num_meas, 1);
    og_sizes{robot_i} = zeros(num_meas, 1);
    mg_sizes{robot_i} = zeros(num_meas, 1);
    for meas_i = 1:num_meas
        meas = accuracy_measurements{meas_i};
        for mg_i = 1:numel(meas.matched_groups)
            if (any(meas.matched_groups(mg_i).members == robot_i))
                mg_sizes{robot_i}(meas_i) = ...
                    meas.matched_groups(mg_i).num_frames;
                break;
            end
        end
        for og_i = 1:numel(meas.optimized_groups)
            if (any(meas.optimized_groups(og_i).members == robot_i))
                robot_accuracies{robot_i}(meas_i) = ...
                    meas.optimized_groups(og_i).ATE;
                og_sizes{robot_i}(meas_i) = ...
                    meas.optimized_groups(og_i).num_frames;
                break;
            end
        end
    end
end
yyaxis left;
ate = plot(times, robot_accuracies{1}, '-');
hold on;
for i = 2:num_robots
    plot(times, robot_accuracies{i}, '-');
end
%orb = plot(xlim, [orb_ate orb_ate], 'b--');
hold off;
ylabel('ATE [m]');
xlabel('time [s]');
grid on;

yyaxis right;
opted = plot(times, og_sizes{1}, '-');
hold on;
for i = 2:num_robots
    plot(times, og_sizes{i}, '-');
end
%for i = 1:num_robots
%    matched = plot(times, mg_sizes{i}, '.');
%end
et = plot([vo_end_time vo_end_time], ylim, 'k--');
hold off;
ylabel('frame count in CC');
title('accuracy development');
set(gca, 'XLim',[min(times) max(times)]);
%legend([ate, opted, matched, et, orb], 'ATE', 'optimized', 'matched', ...
%    'VO end time', 'ORB SLAM','Location', 'NorthWest');
legend(et, 'VO end time', 'Location', 'NorthWest');

end


addpath ../../examples/imagenet;
load('./train_val_test_split.mat') ;
imdb.imageDir = '/home/zzd/Image-Text-Embedding/dataset/Flickr30k-prepare/flickr30k-images-256/'; %please change it to your full path

filename = dir([imdb.imageDir,'*jpg']);
cell_data = {filename(1:end).name}.';
full_path = cellfun(@(x) [imdb.imageDir,x],cell_data,'UniformOutput',false);
imdb.images.data = full_path;
imdb.images.set = set;

count = 0;
for i=1:numel(set)
    if(set(i)==1)
        count = count +1;
        imdb.images.label(i) = single(count);
    else
        imdb.images.label(i) = 0;
    end
end

% Compute image statistics (mean, RGB covariances, etc.)
train = find(imdb.images.set == 1) ;
images = fullfile(imdb.images.data(train(1:end))) ;
[averageImage, rgbMean, rgbCovariance] = getImageStats(images, ...
    'imageSize', [256 256], ...
    'numThreads', 16, ...
    'gpus', 3) ;
%imdb.averageImage = averageImage;
imdb.rgbMean = rgbMean;
imdb.rgbCovariance =rgbCovariance;

save('url_data.mat','imdb');


classdef InertialControlFrame < AbstractControlFrame
    %InertialControlFrame

    properties(Constant)
        enum = ControlFramesEnum.InertialFrame;
    end

    methods
        function obj = InertialControlFrame()

        end

        function enum = getControlFrameEnum(obj)
            enum = ControlFramesEnum.InertialFrame;
        end

        function dcm = computeDcmToInertialFrame(obj, ut, rVect, vVect, bodyInfo, gammaAng, betaAng, alphaAng, baseFrame)
            arguments
                obj(1,1) InertialControlFrame
                ut(1,1) double
                rVect(3,1) double
                vVect(3,1) double
                bodyInfo(1,1) KSPTOT_BodyInfo
                gammaAng(1,1) double
                betaAng(1,1) double
                alphaAng(1,1) double
                baseFrame(1,1) AbstractReferenceFrame
            end

            frame = bodyInfo.getBodyCenteredInertialFrame();
            ce = CartesianElementSet(ut, rVect, vVect, frame);
            ce = ce.convertToFrame(baseFrame);

%             [~, ~, ~, base_frame_2_inertial] = baseFrame.getOffsetsWrtInertialOrigin(ut, ce, baseFrame.getOriginBody());
            base_frame_2_inertial = baseFrame.getRotMatToInertialAtTime(ut, ce, baseFrame.getOriginBody());

            body_2_base_frame = eul2rotmARH([alphaAng,betaAng,gammaAng],'zyx');
            dcm = base_frame_2_inertial * body_2_base_frame;
        end

        function [gammaAngle, betaAngle, alphaAngle] = getAnglesFromInertialBodyAxes(obj, attState, ut, rVect, vVect, bodyInfo, inFrame)
            arguments
                obj(1,1) InertialControlFrame
                attState(1,1) LaunchVehicleAttitudeState
                ut(1,1) double
                rVect(3,1) double
                vVect(3,1) double
                bodyInfo(1,1) KSPTOT_BodyInfo
                inFrame(1,1) AbstractReferenceFrame
            end

            [gammaAngle, betaAngle, alphaAngle] = attState.getEulerAnglesInBaseFrame(ut, rVect, vVect, bodyInfo, inFrame);
        end
    end
end


function h = plotUnitCells(ebsd,d,varargin)
% low level plotting routine for EBSD maps
%

unitCell = ebsd.unitCell;

xy = [ebsd.prop.x(:),ebsd.prop.y(:)];
if check_option(varargin,'region')

  reg = get_option(varargin,'region');

  ind = xy(:,1) > reg(1) & xy(:,1) < reg(2) & xy(:,2) > reg(3) & xy(:,2) < reg(4);

  xy = xy(ind,:);

  if numel(d) == numel(ind) || numel(ind) == size(d,1)
    d = d(ind,:);
  end

end

ax = get_option(varargin,'parent',gca);

if ~isempty(unitCell)

  type = get_flag(varargin,{'unitcell','points','measurements'},'unitcell');

else

  type = 'points';

end

if numel(d) == size(xy,1) || numel(d) == 3*size(xy,1)

  obj.FaceVertexCData = reshape(d,size(xy,1),[]);

  if check_option(varargin,{'transparent','translucent','faceAlpha'})

    s = get_option(varargin,{'transparent','translucent','faceAlpha'},1,'double');
    varargin = delete_option(varargin,'faceAlpha');

    if size(d,2) == 3 % rgb
      obj.FaceVertexAlphaData = s.*(1-min(d,[],2));
    else
      obj.FaceVertexAlphaData = s.*d./max(d);
    end
    obj.AlphaDataMapping = 'none';
    obj.FaceAlpha = 'flat';
  end
  obj.FaceColor = 'flat';
else
  obj.FaceColor = d;
end

obj.EdgeColor = 'none';

switch lower(type)
  case 'unitcell'

    % generate patches
    [obj.Vertices, obj.Faces] = generateUnitCells(xy,unitCell,varargin{:});

  case {'points','measurements'}

    obj.Vertices = xy;
    obj.Faces    = (1:size(xy,1))';

    obj.FaceColor = 'none';
    obj.EdgeColor = 'flat';
    obj.Marker = '.';
    obj.MarkerFaceColor = 'flat';

end

h = optiondraw(patch(obj,'parent',ax),varargin{:});

if ~check_option(varargin,'DisplayName')
  set(get(get(h,'Annotation'),'LegendInformation'),'IconDisplayStyle','off');
end

if nargout == 0, clear h;end


function [val, subCoords] = rmCoordsGet(viewType, model, param, coords)
% rmCoordsGet - wrapper for rmGet to limit values to certain coordinates
%
% val = rmCoordsGet(viewType, model, param, coords)
%

% 2008/02 SOD: split of from rmPredictedTSeries.
% 2008/06 RAS: changed view argument to viewType: this will prevent the
% rmPlotGUI code from having to carry around a large view structure,
% substantially reducing its memory footprint.
% 2009/01 RAS: allows the coords variable for gray views to be a 3xN list
% of coordinates, in addition to a vector of gray node indices.
% 2018/02 RKL: returns the coords as the 2nd output. because they will be a
% subset of the coords that are inputed
if ~exist('viewType','var') || isempty(viewType),   error('Need view type'); end;
if ~exist('model','var')    || isempty(model),      error('Need model');     end;
if ~exist('param','var')    || isempty(param),      error('Need param');     end;
if ~exist('coords','var'),                          error('Need coords');    end;
if isempty(coords), val = []; return; end
% allow the model to be a cell array -- just take the first entry
if iscell(model), model = model{1};  end

% if we input a view instead of a viewType, sort this out
if isstruct(viewType), vw = viewType; viewType = vw.viewType; end

tmp = rmGet(model, param);

switch lower(viewType),
    case 'inplane'

        % normally inplane data will be 3D, but if we have only one slice
        % it will be 2d. we need to check.
        if ~exist('vw', 'var') || isempty(vw), vw = getSelectedInplane; end
        dims = length(viewGet(vw, 'anat size'));

        % If there is one value per voxel, then we expect a matrix of
        % parameter values with the same dimensionality as our inplane
        % anatomy (usually 3d, unless we have only 1 slice in which case it
        % is 2d). So we get the values of this matrix indexed by the 3D
        % coords. So for coord (x, y, z), we would like
        %   val = tmp(x, y, z);
        % In the case of a 2D anat, we would like
        %   val = tmp(x,y);
        if length(size(tmp)) <= dims
            val = zeros(size(coords, 2), 1);
            for n = 1:length(val),
                if dims == 3,
                    val(n) = tmp(coords(1,n), coords(2,n), coords(3,n));
                elseif dims == 2
                    val(n) = tmp(coords(1,n), coords(2,n));
                end
            end;
        end

        % If there is more than one value per voxel, then we expect a
        % matrix of parameter values that has one more dimension than our inplane anatomy.
        % In this case we get all the values of this matrix whose first
        % 3 dimensionpars are indexed by the 3D coords. So for coord (x, y, z)
        % we would like
        %   val = tmp(x, y, z, :);
        % If our inplane happens to be 2d (because we have only slice),
        % then we would like
        %   val = tmp(x, y, :);

        if length(size(tmp)) > dims
            val = zeros(size(coords, 2), size(tmp, dims));
            for n = 1:size(val,1),
                 if dims == 3,
                     val(n, :) = tmp(coords(1,n), coords(2,n), coords(3,n), :);
                 elseif dims == 2
                     val(n, :) = tmp(coords(1,n), coords(2,n), :);
                 end
            end;
        end

    case 'gray'
        % allow 3xN gray coords to be specified, as well as gray node
        % indices:
        if size(coords, 1)==3
            % 3xN coords specification: remap into indices
            vw = getSelectedGray;
            allCoords = viewGet(vw, 'coords');
            [subCoords coords] = intersectCols(allCoords, coords); %#ok<ASGLU>
        end

        if numel(size(tmp)) == 2,
            val = tmp(coords);
        else
            if length(coords)==1
                % the squeeze command will mis-orient fields like beta, by
                % permuting across multiple dimensions. We want it to
                % voxels x predictors.
                val = permute(tmp(1,coords,:), [2 3 1]);
            else
                val = squeeze(tmp(1,coords,:));
            end
        end

    otherwise, error('Invalid view type.')
end;

return


function data=extractdatapt(data,t,offset)
% Extract segements of spike times between t(1) and t(2)
% Usage: data=extractdatapt(data,t,offset)
%
% Input:
% data: structural array of spike times for each channel/trial or a single
%       array of spike times
% t   : time as a 2d vector [t(1) t(2)]
% offset: 0/1 - if 1, store the spike times relative to start of window i.e. t(1)
%         if 0, don't reset the times. Default 0.
% Note that all times can be in arbitrary units. But the units have to be
% consistent. So, if E is in secs, win, t have to be in secs, and Fs has to
% be Hz. If E is in samples, so are win and t, and Fs=1. In case of spike
% times, the units have to be consistent with the units of data as well.
%
% Output:
% data: spike times between t(1) and t(2)
if nargin < 2; error('Need data and times'); end;
if t(1) < 0 || t(2)<=t(1);
    error('times cannot be negative and t(2) has to greater than t(1)');
end;
if nargin < 3 || isempty(offset); offset=0; end;
if isstruct(data);
    C=length(data);
elseif min(size(data))~=1;
    error('Can only accept single vector data unless it is a struct array');
else
    C=1;
    data=change_row_to_column(data);
end;
%fnames=fieldnames(data);
d2(1:C)=struct('times',[]);
for c=1:C,
    if isstruct(data)
       fnames=fieldnames(data);
       eval(['dtmp=data(c).' fnames{1} ';'])
    else
       dtmp=data(:);
    end
%     eval(['dtmp=data(c).' fnames{1} ';' ])
    sp=dtmp(dtmp>=t(1) & dtmp<t(2));
    if offset==1; d2(c).times=sp-t(1);
    else d2(c).times=sp;end
end;
data=d2;


function s = spmatrix(a)
%SPMATRIX Converts a two-way sparse tensor to sparse matrix.
%
%   SPMATRIX(X) converts a sparse tensor to a sparse matrix. The sparse
%   tensor must be two-dimensional.
%
%   See also SPTENSOR, SPTENSOR/RESHAPE, SPTENMAT
%
%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

if ndims(a) ~= 2
    error('Sparse tensor must be two dimensional.');
end

if isempty(a.subs)
    s = sparse(a.size(1), a.size(2));
else
    s = sparse(a.subs(:,1), a.subs(:,2), a.vals, a.size(1), a.size(2));
end


function [h,b] = montage(varargin)
%MONTAGE Display multiple image frames as rectangular montage.
%   MONTAGE displays all the frames of a multiframe image array in a single
%   image object, arranging the frames so that they roughly form a square.
%
%   MONTAGE(I) displays the K frames of the intensity image array I. I is
%   M-by-N-by-1-by-K.
%
%   MONTAGE(BW) displays the K frames of the binary image array BW. BW is
%   M-by-N-by-1-by-K.
%
%   MONTAGE(X,MAP) displays the K frames of the indexed image array X, using the
%   colormap MAP for all frames. X is M-by-N-by-1-by-K.
%
%   MONTAGE(RGB) displays the K frames of the truecolor image array RGB. RGB is
%   M-by-N-by-3-by-K.
%
%   H = MONTAGE(...) returns the handle to the image object.
%
%   Class support
%   -------------
%   An intensity image can be logical, uint8, uint16, int16, single, or double.
%   An indexed image can be logical, uint8, uint16, single, or double. The map
%   must be double. A truecolor image can be uint8, uint16, single, or
%   double. The output is a handle to the graphics objects produced by
%   this function.
%
%   Example
%   -------
%       load mri
%       montage(D,map)
%
%   See also IMMOVIE.

%   Copyright 1993-2005 The MathWorks, Inc.
%   $Revision: 1.1.8.4 $  $Date: 2005/11/15 01:03:29 $

[a,cm,aspectRatio] = parse_inputs(varargin{:});

[nRows, nCols, nBands, nFrames] = size(a);

% Estimate nMontageColumns and nMontageRows given the desired ratio of
% Columns to Rows to be one (square montage).
nMontageCols = sqrt(aspectRatio * nRows * nFrames / nCols);

% Make sure montage rows and columns are integers. The order in the adjustment
% matters because the montage image is created horizontally across columns.
nMontageCols = ceil(nMontageCols);
nMontageRows = ceil(nFrames / nMontageCols);

% Create the montage image.
b = a(1,1); % to inherit type
b(1,1) = 0; % from a
b = repmat(b, [nMontageRows*nRows, nMontageCols*nCols, nBands, 1]);

rows = 1 : nRows;
cols = 1 : nCols;

for i = 0:nMontageRows-1
  for j = 0:nMontageCols-1,
    k = j + i * nMontageCols + 1;
    if k <= nFrames
      b(rows + i * nRows, cols + j * nCols, :) = a(:,:,:,k);
    else
      break;
    end
  end
end

if isempty(cm)
  hh = imshow(b);
else
  hh = imshow(b,cm);
end

if nargout > 0
    h = hh;
end

%--------------------------------------------------------------
%Parse Inputs Function

function [I,map,aspectRatio] = parse_inputs(varargin)

% initialize variables
map = [];
aspectRatio = 1;

iptchecknargin(1,4,nargin,mfilename);
iptcheckinput(varargin{1},{'uint8' 'double' 'uint16' 'logical' 'single' ...
                    'int16'},{},mfilename, 'I, BW, or RGB',1);
I = varargin{1};

if nargin>=2
  if isa(I,'int16')
    eid = sprintf('Images:%s:invalidIndexedImage',mfilename);
    msg1 = 'An indexed image can be uint8, uint16, double, single, or ';
    msg2 = 'logical.';
    error(eid,'%s %s',msg1, msg2);
  end
  if nargin >= 3 & isstr(varargin{2}),
    if strcmp(varargin{2},'aspectratio'),
      aspectRatio = varargin{3};
    end;
  else,
    map = varargin{2};
    iptcheckinput(map,{'double'},{},mfilename,'MAP',1);
    if ((size(map,1) == 1) && (prod(map) == numel(I)))
      % MONTAGE(D,[M N P]) OBSOLETE
      eid = sprintf('Images:%s:obsoleteSyntax',mfilename);
      msg1 = 'MONTAGE(D,[M N P]) is an obsolete syntax.';
      msg2 = 'Use multidimensional arrays to represent multiframe images.';
      error(eid,'%s\n%s',msg1,msg2);
    end
  end
end;


function result = calcMapTopkMapTopkPreTopkRecLabel(queryLabel, retrievalLabel, qB, rB, topk)
%% Function: calcMapTopkMapTopkPreTopkRecLabel:
%   calculate Map, Topk map, Topk precision, Topk recall for hamming ranking task.
% Input:
%   queryLabel: 0-1 label matrix (numQuery * numLabel) for query set.
%   retrievalLabel: 0-1 label matrix (numQuery * numLabel) for retrieval set.
%   qB: compressed binary code for query set.
%   rB: compressed binary code for retrieval set.
%   topk (optional): vector for different (non-zero and ascending) topk.
% Output:
%   result.map: map for whole retrieval set
%   result.topkMap: vector. topk-Map for different topk
%   result.topkPre: vector. topk-Precision for different topk
%   result.topkRec: vector. topk-Recall for different topk

flag = false;
if exist('topk', 'var')
    flag = true;
end

numQuery = size(qB, 1);
map = 0;
if flag
    nt = numel(topk);
    topkPre = zeros(1, nt);
    topkRec = zeros(1, nt);
    topkMap = zeros(1, nt);
end

for ii = 1: numQuery
    gnd = queryLabel(ii, :) * retrievalLabel' > 0;
    tsum = sum(gnd);
    if tsum == 0
        continue;
    end
    hamm = hammingDist(qB(ii, :), rB);
    [~, index] = sort(hamm);
    gnd = gnd(index);
    count = 1: tsum;
    tindex = find(gnd == 1);
    map = map + mean(count ./ tindex);
    if flag
        for jj = 1: nt
            tgnd = gnd(1: topk(jj));
            if sum(tgnd) == 0
                continue;
            end
            tcount = 1: sum(tgnd);
            tindex = find(tgnd == 1);
            topkMap(jj) = topkMap(jj) + mean(tcount ./ tindex);
            topkPre(jj) = topkPre(jj) + sum(tgnd) / topk(jj);
            topkRec(jj) = topkRec(jj) + sum(tgnd) / tsum;
        end
    end
end

result.map = map / numQuery;
if flag
    result.topkMap = topkMap / numQuery;
    result.topkPre = topkPre / numQuery;
    result.topkRec = topkRec / numQuery;
end
end


clc;
clear all;

toolboxpath = '/home/siyuan/Documents/Dataset/SUNRGBD_ALL/SUNRGBDtoolbox/SUNRGBDtoolbox';
addpath(genpath(toolboxpath));
load(fullfile(toolboxpath,'/traintestSUNRGBD/allsplit.mat'));
load(fullfile('SUNRGBDMetaUS.mat'));
new_3d = load('SUNRGBDMeta3DBB_v2.mat');
cls = {'bathtub','bed','bookshelf','box','chair','counter','desk','door','dresser','garbage_bin','lamp','monitor','night_stand','pillow','sink','sofa','table','tv','toilet'};
thresh_iou = [0.01,0.05:0.05:1.6,0.8:0.2:1];
result_path = '/home/siyuan/Documents/cvpr2018/evaluation';
vis = true;

%% evaluation
iou_precision =[];
iou_recall =[];
label_precision =[];
iouHolistic_all = [];
for imageId = 11
    % fake result always use same room and object boxes
    data = SUNRGBDMeta(imageId);
    bdb_3d = load(fullfile(result_path, int2str(imageId), 'bdb_3d.mat'));
    bdb = bdb_3d.bdb;
    layout = load(fullfile(result_path, int2str(imageId), 'layout.mat'));
    predictRoom = layout.layout;
    gt_bdb = new_3d.SUNRGBDMeta(imageId).groundtruth3DBB;
    % evaluate
    for i = 1:size(bdb, 1)
    	predictedBbs(i) = create_bounding_box_3d([bdb(i, 9), bdb(i, 10); bdb(i, 11), bdb(i, 12)], [bdb(i, 3), bdb(i, 4), bdb(i, 5)], [bdb(i, 6), bdb(i, 7), bdb(i, 8)]);
    end
    eval_result = eval_holisticScencePR(predictedBbs',gt_bdb',thresh_iou,cls);
    iouHolistic = eval_holisticScenceIoU(data,data.gtCorner3D,gt_bdb',predictRoom,predictedBbs',vis);

    iou_precision = [iou_precision;eval_result.iou_precision];
    iou_recall = [iou_recall;eval_result.iou_recall];
    label_precision = [label_precision;eval_result.label_precision];
    iouHolistic_all = [iouHolistic_all iouHolistic];
end
%% plot and average
figure;
thresh_iou = [0.01,0.05:0.05:1.6,0.8:0.2:1];
load('iou_precision.mat');
load('iou_recall.mat');
load('iouHolistic_all.mat');
plot(thresh_iou,mean(iou_precision),'r','LineWidth',3);
hold on;
plot(thresh_iou,mean(iou_recall),'g','LineWidth',3);
plot(thresh_iou,mean(label_precision),'b','LineWidth',3);
legend({'geometric precision','geometric recall','recognition recall'},'FontSize',27)
%xlabel('IoU threshold');
axis([0 1 0 1])
iou_precision_average =  mean(iou_precision(:,6));
iou_recall_average =  mean(iou_recall(:,6));
label_precision_average =  100*mean(label_precision(:,6));
iouHolistic_average = mean(iouHolistic_all);
sprintf('Average:\n geometric precision: %.3f\n geometric recall: %.3f\n recognition recall: %.3f\n %.3f\n',...
    iou_precision_average,iou_recall_average,label_precision_average,iouHolistic_average)


 function y = mtimes(ob, x)
%function y = mtimes(ob, x)	for Gnufft object
% y = G * x	or x = G' * y

if ob.apower ~= 1, error 'power not done', end

%
% partial projection or backprojection (for ordered subsets)
%
if ob.is_subref
	error 'subref not done'

%
% full "projection"
%
elseif ~ob.is_transpose
	if ob.is_masked
		x = embed(x, ob.mask);
	end

	%
	% full dsft, which doesn't have shift implemented anyway
	%
%	if ob.is.dsft
%		x = reshape(x, ob.Nd);
%		y = dtft_mex('forward', ob.omega', x, int32(ob.nthread));

	%
	% nufft
	%
%	else
		x = reshape(x, [ob.Nd numel(x)/prod(ob.Nd)]);
		y = nufft(x, ob.st);
%	end

%
% full "back-projection"
%
else

%	if ob.is.dsft
%		if isreal(y)
%			y = complexify(y);
%			warning 'faking complex'
%		end
%		y = dtft_mex('adjoint', ob.omega', y, ...
%			int32([ob.nx ob.ny]'), int32(ob.nthread));
%		y = ob.pixel_size * y;

%	else
		y = nufft_adj(x, ob.st);
%	end

	if ob.is_masked
		y = y(ob.mask);
	end

	y = reshape(y, [ob.dims(1) numel(y) / ob.dims(1)]); % [*Nd,L]
end


function p = cleanuppermodel(p)

% We might have created a bilinear model from an original polynomial model.
% We should use the original model when we solve the upper bound problem.
p_bilinear = p;
p = p.originalModel;

p = removeCuts(p);

n_start = length(p.c);

% Quadratic mode, and quadratic aware solver?
bilinear_variables = find(p.variabletype == 1 | p.variabletype == 2);
if ~isempty(bilinear_variables)
    used_in_c = find(p.c);
    quadraticterms = used_in_c(find(ismember(used_in_c,bilinear_variables)));
    if ~isempty(quadraticterms) & p.solver.uppersolver.objective.quadratic.nonconvex
        usedinquadratic = zeros(1,length(p.c));
        for i = 1:length(quadraticterms)
            Qij = p.c(quadraticterms(i));
            power_index = find(p.monomtable(quadraticterms(i),:));
            if length(power_index) == 1
                p.Q(power_index,power_index) = Qij;
            else
                p.Q(power_index(1),power_index(2)) = Qij/2;
                p.Q(power_index(2),power_index(1)) = Qij/2;
            end
            p.c(quadraticterms(i)) = 0;
        end
    end
end

p.lb = p_bilinear.lb(1:length(p.c));
p.ub = p_bilinear.ub(1:length(p.c));
p.bilinears = [];


function visualize_approximate_dynamic_images(images)
% VISUALIZE_DYNAMIC_IMAGES

di = compute_approximate_dynamic_images(images) ;

di = di - min(di(:)) ;
di = 255 * di ./ max(di(:)) ;
image(uint8(di)) ;


% Loads the true SST fields from the HadGem1 runs, the input
% is the name of the file name with the training data
%
function [ true_sst, Itest, Itrain, time, train_sst ] = ...
    load_testdata( dataset )

switch dataset
case 'hadsst2d1'
    datafile = 'run1_ts_A1_5x5';
case 'hadsst2d2'
    datafile = 'run2_ts_A1_5x5';
end

% Load training data
datapath = metoffice_get_path();
train = load( [ datapath, dataset ], 'time', 'sst' );
%train = load( [ datapath '/RecTest/DATASETS/' dataset ], 'time', 'sst' );
Itrain = ~isnan( train.sst );

% Load test data
testpath = metoffice_get_testpath();
hadgem = load( [ testpath, datafile ], 'sst', 'time' );
% $$$ hadgem = load( [ datapath '/HadGem1/' datafile ], 'sst', 'time' );

[ time, Ihadsst, Ihadgem ] = intersect( train.time, hadgem.time );

% Take the time period included in the test data
true_sst = hadgem.sst(:,Ihadgem);

% Load mask
[ mask, lat, lon ] = metoffice_get_mask( dataset );

Itest = logical( ones(size(true_sst)) );
Itest( ~mask(:), : ) = 0;
Itest( Itrain ) = 0;
% Possible missing data in model runs
Itest( isnan(true_sst) ) = 0;
% Remove ice from test data
Iice = true_sst < -1.8;
Itest(Iice) = 0;

% Remove Caspian sea (detached area with no observations)
mask_Caspian = zeros(length(lat),length(lon));
%          lat  lon
ilat = find(lat==47.5);
ilon = find(lon==52.5);
mask_Caspian( ilat, ilon ) = 1;
mask_Caspian( ilat+1, ilon ) = 1;
mask_Caspian( ilat+2, ilon ) = 1;
mask_Caspian( ilat, ilon-1 ) = 1;
mask_Caspian = reshape( mask_Caspian, length(lat)*length(lon), 1 );
%lmap( mask_Caspian, lat, lon );

Itest( logical(mask_Caspian), : ) = 0;

%true_sst(~(Itest | Itrain)) = NaN;

if nargout >= 5
    train_sst = train.sst(:,Ihadsst);
end


classdef opf_model < opt_model
%OPF_MODEL  Constructor for OPF model class.
%   OM = OPF_MODEL(MPC)
%
%   This class implements the OPF model object used to encapsulate
%   a given OPF problem formulation. It allows for access to optimization
%   variables, constraints and costs in named blocks, keeping track of the
%   ordering and indexing of the blocks as variables, constraints and costs
%   are added to the problem.
%
%   This class is a subclass of OPT_MODEL that adds the 'mpc'
%   field for storing the MATPOWER case struct used to build the object
%   along with the get_mpc() method.
%
%   It also adds the 'cost' field and the following three methods for
%   implementing the legacy user-defined OPF costs:
%       add_legacy_cost
%       params_legacy_cost
%       eval_legacy_cost
%
%   The following is the structure of the data in the OPF model object.
%
%   om
%       <opt_model fields> - see OPT_MODEL for details
%       .cost       - data for legacy user-defined costs
%           .idx
%               .i1 - starting row index within full N matrix
%               .iN - ending row index within full N matrix
%               .N  - number of rows in this cost block in full N matrix
%           .N      - total number of rows in full N matrix
%           .NS     - number of cost blocks
%           .data   - data for each user-defined cost block
%               .N  - see help for ADD_LEGACY_COST for details
%               .H  -               "
%               .Cw -               "
%               .dd -               "
%               .rr -               "
%               .kk -               "
%               .mm -               "
%               .vs - cell array of variable sets that define xx for this
%                     cost block, where the N for this block multiplies xx
%           .order  - struct array of names/indices for cost blocks in the
%                     order they appear in the rows of the full N matrix
%               .name   - name of the block, e.g. R
%               .idx    - indices for name, {2,3} => R(2,3)
%       .mpc        - MATPOWER case struct used to create this model object
%           .baseMVA
%           .bus
%           .branch
%           .gen
%           .gencost
%           .A  (if present, must have l, u)
%           .l
%           .u
%           .N  (if present, must have fparm, H, Cw)
%           .fparm
%           .H
%           .Cw
%
%   See also OPT_MODEL.

%   MATPOWER
%   Copyright (c) 2008-2022, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

    properties
        cost = [];
        mpc = struct();
    end     %% properties

    methods
        %% constructor
        function om = opf_model(mpc)
            args = {};
            have_mpc = 0;
            if nargin > 0
                if isa(mpc, 'opf_model')
                    args = { mpc };
                elseif isstruct(mpc)
                    have_mpc = 1;
                end
            end

            %% call parent constructor
            om@opt_model(args{:});

            if have_mpc
                om.mpc = mpc;
            end

            %% Due to a bug related to inheritance in constructors in
            %% Octave 5.2 and earlier (https://savannah.gnu.org/bugs/?52614),
            %% INIT_SET_TYPES() cannot be called directly in the
            %% MP_IDX_MANAGER constructor, as desired.
            %%
            %% WORKAROUND:  INIT_SET_TYPES() is called explicitly as needed
            %%              (if om.var is empty) in ADD_VAR(), DISPLAY() and
            %%              INIT_INDEXED_NAME(), after object construction,
            %%              but before object use.
        end

        function om = def_set_types(om)
            om.set_types = struct(...
                    'var',  'VARIABLES', ...
                    'lin',  'LINEAR CONSTRAINTS', ...
                    'nle',  'NONLIN EQ CONSTRAINTS', ...
                    'nli',  'NONLIN INEQ CONSTRAINTS', ...
                    'qdc',  'QUADRATIC COSTS', ...
                    'nlc',  'GEN NONLIN COSTS', ...
                    'cost', 'LEGACY COSTS'  ...
                );
        end

        function om = init_set_types(om)
            %% call parent to create base data structures for each type
            init_set_types@opt_model(om);

            %% finish initializing data structures for each type
            es = struct();  %% empty struct
            om.cost.data = struct( ...
                'N', es, ...
                'H', es, ...
                'Cw', es, ...
                'dd', es, ...
                'rh', es, ...
                'kk', es, ...
                'mm', es, ...
                'vs', es );
            om.cost.params = [];
        end
    end     %% methods
end         %% classdef


% ChromosomePositionHistogram
% NOTE: Histograms for RNA Polymerase
% Author: Derek Macklin, macklin@stanford.edu
% Affiliation: Covert Lab, Department of Bioengineering, Stanford University
% Created: 8/18/2011
classdef ChromosomePositionHistogram
    properties (Constant)
        boundIndices = struct( ...
            'complex', {{...
                82, ...                                             % SMC
                50, ...                                             % Helicase
                [180, 181, 183, 185, 187, 189, 191, 193], ...       % DnaA
                1, ...                                              % Gyrase
                78, ...                                             % Topo IV
                165, ...                                            % LuxR
                }}, ...
            'monomer', {{
                101, ...                                            % HTH regulator
                242, ...                                            % Ferric uptake repressor
                }}...
            );
        boundIndicesNames = struct( ...
            'complex', {{...
                'SMC', ...
                'Helicase', ...
                'DnaA', ...
                'Gyrase', ...
                'Topo IV', ...
                'LuxR'
                }}, ...
            'monomer', {{
                'HTH regulator', ...
                'Ferric uptake\nrepressor', ...
                }}...
            );
    end

    methods (Static = true)
        function run(simDir, fileName)
            import edu.stanford.covert.cell.sim.util.SimulationDiskUtil;
            import edu.stanford.covert.cell.sim.util.DiskLogger;
            import edu.stanford.covert.cell.sim.util.PlotUtil;
            import edu.stanford.covert.cell.sim.analysis.ChromosomePositionHistogram;

            %% options
            if nargin < 1 || isempty(simDir)
                simDir = SimulationDiskUtil.getLatestSimulation();
            end

            [simDir, ~, sim] = SimulationDiskUtil.getSimulation(simDir);
            c = sim.state('Chromosome');

            %% get data
            stateNames = {
                'ProteinComplex' 'counts'
                'Chromosome' 'complexBoundSites'
                'Chromosome' 'monomerBoundSites'
                'RNAPolymerase' 'positionStrands'
                'RNAPolymerase' 'states'
                'Time' 'values'};
            states = DiskLogger.load(simDir, stateNames, [], [], [], 'extract');

            cDensityMatrix = ChromosomePositionHistogram.makeDensityMatrix(states, ChromosomePositionHistogram.boundIndices, sim);

            %% figure 1
            [~, figHandle1] = edu.stanford.covert.cell.sim.util.PlotUtil.newAxesHandle();
            clf(figHandle1);
            nPlots = numel(ChromosomePositionHistogram.boundIndices.complex);
            [axesHandles, xAxesHandle] = PlotUtil.multiElementPlot(...
                figHandle1, 3.5 * ones(nPlots, 1), [1 c.sequenceLen], ...
                struct(...
                'titleStr', 'Protein Density on Chromosome 1', ...
                'xlabelStr', 'Position on Chromosome 1'));
            set(xAxesHandle, 'XTick', [1 c.terCPosition c.sequenceLen], 'XTickLabel', {'oriC', 'terC', 'oriC'});

            for i = 1:size(cDensityMatrix, 2)
                PlotUtil.plotLine(axesHandles(i), 1:size(cDensityMatrix, 1), cDensityMatrix(:, i), false, true, false);
                ylabel(axesHandles(i), {ChromosomePositionHistogram.boundIndicesNames.complex{i} '(Counts)'});
            end
            if exist('fileName', 'var') && ~isempty(fileName)
                saveas(figHandle1, sprintf('%s-1.pdf', fileName));
                close(figHandle1);
            end

            %% figure 2
            [~, figHandle2] = edu.stanford.covert.cell.sim.util.PlotUtil.newAxesHandle();
            ChromosomePositionHistogram.plotRnaPolymeraseDensity(sim, states, figHandle2);

            if exist('fileName', 'var') && ~isempty(fileName)
                saveas(figHandle2, sprintf('%s-2.pdf', fileName));
                close(figHandle2);
            end
        end
    end

    %multiple simulations
    methods (Static = true)

        % TODO: Handle cases where we don't have one of monomers or complexes
        function [cDensityMatrix, mDensityMatrix] = makeDensityMatrix(states, boundIndices, simulation)
            complexIdxs = boundIndices.complex;
            monomerIdxs = boundIndices.monomer;

            cbs = states.Chromosome.complexBoundSites;
            mbs = states.Chromosome.monomerBoundSites;

            ch = simulation.state('Chromosome');

            cDensityMatrix = zeros(size(cbs, 1), numel(complexIdxs));
            for i = 1:numel(complexIdxs)
                ftpt = ones(ch.complexDNAFootprints(complexIdxs{i}(1)), 1);
                for j = 1:numel(complexIdxs{i})
                    complexIdx = complexIdxs{i}(j);
                    cDensityMatrix(:, i) = cDensityMatrix(:, i) + ...
                        cconv(...
                        full(sum(sum(cbs(:, 1:2, :) == complexIdx, 3), 2)), ...
                        ftpt, ...
                        size(cDensityMatrix, 1) ...
                        );
                end
            end
            cDensityMatrix = cDensityMatrix / size(cbs, 3);

            mDensityMatrix = zeros(size(mbs, 1), numel(monomerIdxs));
            for i = 1:numel(monomerIdxs)
                ftpt = ones(ch.monomerDNAFootprints(monomerIdxs{i}(1)), 1);
                for j = 1:numel(monomerIdxs{i})
                    monomerIdx = monomerIdxs{i}(j);
                    mDensityMatrix(:, i) = mDensityMatrix(:, i) + ...
                        cconv(...
                        full(sum(sum(mbs(:, 1:2, :) == monomerIdx, 3), 2)), ...
                        ftpt, ...
                        size(mDensityMatrix, 1) ...
                        );
                end
            end
            mDensityMatrix = mDensityMatrix / size(mbs, 3);

            % Hack to deal with rounding issues that lead to small
            % negative numbers
            cDensityMatrix = max(0, cDensityMatrix);
            mDensityMatrix = max(0, mDensityMatrix);
        end

        function plotRnaPolymeraseDensity(sim, states, figHandle)
            import edu.stanford.covert.cell.sim.util.DiskLogger;
            import edu.stanford.covert.cell.sim.util.PlotUtil;
            import edu.stanford.covert.cell.sim.util.SimulationDiskUtil;
            import edu.stanford.covert.util.CircularSparseMat;

            c = sim.state('Chromosome');
            pol = sim.state('RNAPolymerase');
            pc = sim.state('ProteinComplex');

            nTimePoints = size(states.RNAPolymerase.positionStrands, 3);
            nRnaPol = size(states.RNAPolymerase.positionStrands, 1);
            polStates = reshape(states.RNAPolymerase.states, [nRnaPol * nTimePoints 1]);
            posStrndTimes = reshape(permute([states.RNAPolymerase.positionStrands repmat(permute(1:nTimePoints, [1 3 2]), [nRnaPol 1])], [1 3 2]), [nRnaPol * nTimePoints 3]);

            polStates = polStates(posStrndTimes(:, 1) ~= 0, :);
            posStrndTimes = posStrndTimes(posStrndTimes(:, 1) ~= 0, :);

            % Get rid of strands 3 and 4 (i.e., only look at Chromosome 1)
            polStates = polStates(posStrndTimes(:, 2) <= 2, :);
            posStrndTimes = posStrndTimes(posStrndTimes(:, 2) <= 2, :);

            boundSites = CircularSparseMat(posStrndTimes, polStates, [c.sequenceLen 4 nTimePoints], 1);

            activeDensity = cconv(...
                full(sum(sum(boundSites >= pol.activelyTranscribingValue, 3), 2)), ...
                ones(c.complexDNAFootprints(pc.getIndexs('RNA_POLYMERASE')), 1), ...
                c.sequenceLen) / nTimePoints;
            nonSpecBoundDensity = cconv(...
                full(sum(sum(boundSites == pol.nonSpecificallyBoundValue, 3), 2)), ...
                ones(c.complexDNAFootprints(pc.getIndexs('RNA_POLYMERASE')), 1), ...
                c.sequenceLen) / nTimePoints;
            specBoundDensity = cconv(...
                full(sum(sum(boundSites == pol.specificallyBoundValue, 3), 2)), ...
                ones(c.complexDNAFootprints(pc.getIndexs('RNA_POLYMERASE')), 1), ...
                c.sequenceLen) / nTimePoints;

            % Hack to deal with rounding issues that lead to small
            % negative numbers
            activeDensity = max(0, activeDensity);
            nonSpecBoundDensity = max(0, nonSpecBoundDensity);
            specBoundDensity = max(0, specBoundDensity);

            %layout axes
            clf(figHandle);
            [axesHandles, xAxesHandle] = PlotUtil.multiElementPlot(figHandle, 8 * ones(3, 1), [1 c.sequenceLen], struct( ...
                'titleStr', 'RNA Polymerase Density on Chromosome 1', ...
                'xlabelStr', 'Position on Chromosome 1'));
            set(xAxesHandle, 'XTick', [1 c.terCPosition c.sequenceLen], 'XTickLabel', {'oriC', 'terC', 'oriC'});

            %plot data
            PlotUtil.plotLine(axesHandles(1), 1:c.sequenceLen, activeDensity, false, true, false);
            ylabel(axesHandles(1), 'Active');

            PlotUtil.plotLine(axesHandles(2), 1:c.sequenceLen, specBoundDensity, false, true, false);
            ylabel(axesHandles(2), {'Specifically' 'Bound'});

            PlotUtil.plotLine(axesHandles(3), 1:c.sequenceLen, nonSpecBoundDensity, false, true, false);
            ylabel(axesHandles(3), {'Non-' 'Specifically' 'Bound'});

            % Format Y-axis
            PlotUtil.alignYAxesLabels(axesHandles);
            PlotUtil.offsetYAxes(axesHandles, 0.015);
        end
    end
end


function v = cvx_vexity( x )

global cvx___
s = x.size_;
if any( s == 0 ),
    v = cvx_zeros( s );
    return
end
p  = cvx___.vexity;
b  = x.basis_;
n  = length( p );
nb = size( b, 1 );
if nb < n,
    p = p( 1 : nb, : );
elseif n < nb,
    p( n+1:nb, : ) = 0;
end
b = b( p ~= 0, : );
if isempty( b ),
    v = cvx_zeros( x.size_ );
    if x.slow_,
        v( isnan( x.basis_( 1, : ) ) ) = NaN;
    end
    return
end
p = nonzeros(p).';
if cvx___.nan_used,
    b = sparse( b );
end
v = full( p * b );
tt = abs( v ) ~= abs( p ) * abs( b );
if x.slow_,
    v( tt | isnan( x.basis_( 1, : ) ) ) = NaN;
else
    v( tt ) = NaN;
end
if ~isreal( x ),
    v( any( imag( x.basis_ ), 1 ) & v ) = NaN;
end
v = sign( v );
v = reshape( v, x.size_ );

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


function RM = fmri_mrestriction(TestType,nH, nC, AC, CC, nHTest)
%
% RM = fmri_mrestriction(TestType, nH, nC, AC, CC, <nHTest>)
%
% Creates a restriction matrix
%
% TestType - String (case insensitive) indicating the test to perform:
%   t  - t-Test, RM will be a row vector
%   tm - t-Test, different p for each delay point (RM same as Fc)
%   Fm - F-Test, different p for each delay point
%   F0 - RM same as t-Test
%   Fd - F-test, different row for each delay, all conditions
%        for each delay on the same row
%   Fc - F-test, different row for each condition, all delays
%        for a condition on the same row
%   Fcd - F-test, different row for each condition for each delay
% nH - total number of elements in HDIR
% nC - total number of stimulus conditions (including fixation)
% AC - list of active conditions
% CC - list of control conditions
% nHTest - list of HDIR components to test (default: all).
%
% Eg: fmri_mrestriction('t',10, 6, [1 3], [2 5], [3 6:9])
% Generates an RM for testing conditions (1+3)-(2+5) using
% components 3,6,7,8, and 9 in the HDIR.  Conditions 0 and 4
% are not tested. There are 10 components in the HDIR, and 5
% stimulus conditions.
%
%
% $Id: fmri_mrestriction.m,v 1.1 2004/03/11 01:30:17 sayres Exp $

% Check for the correct number of arguments %
if(nargin ~= 5 & nargin ~= 6)
  msg = 'USAGE: RM = fmri_mrestriction(TestType, nH, nC, AC, CC, <nHTest>';
  qoe(msg);error(msg);
end

% Check the Test Type %
if( isempty( strmatch(upper(TestType),{'T ','TM','FM','F0','FD','FC','FCD','FDC'},'exact')))
  msg = sprintf('Unkown TestType %s',TestType);
  qoe(msg);error(msg);
end

%% check that the input parameters are correct %%
if(nH < 1)
  msg = 'nH must be greater than 0';
  qoe(msg);error(msg);
end
if(nC < 1)
  msg = 'Number of conditions must be greater than 0';
  qoe(msg);error(msg);
end
if(length(find(AC<0 | AC > nC)))
  msg = 'Invalid condition number in AC';
  qoe(msg);error(msg);
end
if(length(find(CC<0 | CC > nC)))
  msg = 'Invalid condition number in CC';
  qoe(msg);error(msg);
end

% Test that the same condition is not in both AC and CC %
C = [AC CC];
for n = 1:nC,
  if(length(find(C==n)) > 1)
    msg = 'Same condition is in both AC and CC';
    qoe(msg);error(msg);
  end
end

% Set defaults for nHTest, if needed, else test range %
if(nargin == 5) nHTest = [1:nH];
else
  if(length(find(nHTest<1 | nHTest > nH)))
    msg = 'Invalid nHTest Component';
    qoe(msg);error(msg);
  end
end

% Strip Condition 0 from AC (if there) %
iACnz = find(AC ~= 0);
if( ~ isempty(iACnz) )
  AC = AC(iACnz);
end
% Strip Condition 0 from CC (if there) %
iCCnz = find(CC ~= 0);
if( ~ isempty(iCCnz) )
  CC = CC(iCCnz);
end

%% -------- Generate the restriction matrix -------- %%
switch(upper(TestType))
  case {'T','F0'},
    RM = fmri_vrestriction(nH,nC,AC,CC,nHTest);
  case {'FD','TM','FM'},
    for n = 1:length(nHTest),
      RV = fmri_vrestriction(nH,nC,AC,CC,nHTest(n));
      RM(n,:) = reshape(RV', [1 prod(size(RV))]);
    end
  case {'FC'},
    for n = 1:length(iACnz),
      RV = fmri_vrestriction(nH,nC,AC(n),0,nHTest);
      RM(n,:) = reshape(RV', [1 prod(size(RV))]);
    end
    for n = 1:length(iCCnz),
      RV = fmri_vrestriction(nH,nC,0,CC(n),nHTest);
      RM(n+length(AC),:) = reshape(RV', [1 prod(size(RV))]);
    end
  case {'FCD','FDC'},
    RM = [];
    for n = 1:length(iACnz),
      RV = fmri_mrestriction('Fd',nH,nC,AC(n),0,nHTest);
      RM = [RM; RV;];
    end
    for n = 1:length(iCCnz),
      RV = fmri_mrestriction('Fd',nH,nC,0,CC(n),nHTest);
      RM = [RM; RV;];
    end
end

return;


function [align] = flydra(in,config)

Cst = in.Cst;
Rot = in.Rot;

Octave = exist('OCTAVE_VERSION', 'builtin') ~= 0;
if ~Octave,
  drawscene(in.Xe,Cst',Rot,41,'cloud','Graphical Output Validation: View from top or bottom (no sRt)',config.cal.cams2use);
end

% definition of the absolute world frame (in cm)

%% for hummingbird cage
%
%cam(3).C = [168, 173, 185]';
%cam(4).C = [142, 76, 89]';
%cam(1).C = [81, 72, 165]';
%cam(2).C = [22,  15, 176]';
%cam(5).C = [53.5, 175, 78]';

% definition of the absolute world frame (in mm) (Doug's cube)

% cam(1).C = [10, 390, 600]';
% cam(2).C = [460,  380, 600]';
% cam(3).C = [430, 115, 580]';
% cam(4).C = [0, 0, 610]';
% cam(5).C = [240, 150, 610]';

% definition of the absolute world frame (in mm) (windtunnel June 2005)

%cam(1).C = [1510, 180, 500+322]';
%cam(2).C = [1140,  260, 450+322]';
%cam(3).C = [1040, 25, 480+322]';
%cam(4).C = [490, 160, 390+322]';
%cam(5).C = [900, 25, 400+322]';

% the above numbers (from June 2005) transformed to acheive better
% agreement (July 3, 2005)

%cam(1).C = [1585.0266, 157.1808, 725.3687]';
%cam(2).C = [1221.0045, 230.7819, 717.1524]';
%cam(3).C = [1123.2774, -2.1424, 731.8758]';
%cam(4).C = [580.8019, 124.5745, 705.3737]';
%cam(5).C = [978.8653, 3.3878, 663.1311]';

% cal July 5, 2005

%cam(1).C = [1710, 375, 820]';
%cam(2).C = [1110, 316, 800]';
%cam(3).C = [1110, 100, 466]';
%cam(4).C = [-380, 290, 875]';
%cam(5).C = [440, 15, 680]';

% touch-up July 6, 2005 (5c)

%cam(1).C = [1853.5325, 228.1160, 971.5365]';
%cam(2).C = [1202.0386, 199.5437, 953.7752]';
%cam(3).C = [1183.3920, 41.6797, 537.2034]';
%cam(4).C = [-406.8365, 229.6378, 1073.0449]';
%cam(5).C = [460.2840, -60.0974, 778.1218]';

% touch-up Aug 22, 2005 (touch-up from A applied to B)

%cam(1).C = cam(1).C - [130.0*0.83, 0., 110.0*0.83]';
%cam(2).C = cam(2).C - [130.0*0.83, 0., 110.0*0.83]';
%cam(3).C = cam(3).C - [130.0*0.83, 0., 110.0*0.83]';
%cam(4).C = cam(4).C - [130.0*0.83, 0., 110.0*0.83]';
%cam(5).C = cam(5).C - [130.0*0.83, 0., 110.0*0.83]';

% touch-up Jan 27, 2006 ( hacked-in estimate )

%cam(1).C = cam(1).C - [130.0*0.83+20.0, 0., 110.0*0.83]';
%cam(2).C = cam(2).C - [130.0*0.83+10.0, 0., 110.0*0.83]';
%cam(3).C = cam(3).C - [130.0*0.83+10.0, 0., 110.0*0.83]';
%cam(4).C = cam(4).C - [130.0*0.83, 0., 110.0*0.83]';
%cam(5).C = cam(5).C - [130.0*0.83, 0., 110.0*0.83]';

% new Feb 14, 2006

%cam(1).C = [1760, 190, 850]';
%cam(2).C = [1060, 260, 785]';
%cam(3).C = [1070, 70, 470 ]';
%cam(4).C = [-380, 230, 873]';
%cam(5).C = [380, -10, 670]';

% moved cam 2006 03 07 19:45

%cam(1).C = [1760, 190, 850]';
%cam(2).C = [1060, 260, 785]';
%cam(3).C = [1070, 190, 510 ]';
%cam(4).C = [-380, 230, 873]';
%cam(5).C = [380, 120, 670]';

if 1,

% 2006 03 31

%cam(1).C = [640, -105, 170]';
%cam(2).C = [1220, 172, 912]';
%cam(3).C = [475, 172, 580 ]';
%cam(4).C = [-350, 172, 932]';
%cam(5).C = [320, -105, 170]';

% 2006 04 03e

%cam(3).C = [380, 240, 530 ]';

% 2006 04 04a

%cam(4).C = [-115, 220, 800]';
end

% 2006 09 14

%cam(1).C = [685, 220, 775]';
%cam(2).C = [1220, 172, 912]';
%cam(3).C = [380, 240, 530 ]';
%cam(4).C = [-115, 220, 800]';
%cam(5).C = [180, 240, 740]';

%% 2006 10 23 (from DLT)
%cam(1).C = [ 651.7963301   169.95615265  749.51356735]';
%cam(2).C = [ 1090.40810501   190.82014771   866.8980674 ]';
%cam(3).C = [ 327.41874025  237.43022637  539.80942371]';
%cam(4).C = [-241.364468    194.27934314  767.35249515]';
%cam(5).C = [ 147.20028399  171.45774441  725.08522715]';

% 2006 12 19 riverside hummingbirds
% estimates from DLT
%cam(1).C = [976.22314128 -2289.85157108   489.78104927]';
%cam(2).C = [-17278.94752856   1794.85211148  15855.51116479]';
%cam(3).C = [ 2266.45466953  2590.2964507    -74.53159994]';
%cam(4).C = [-329.01069449  796.46308431  624.04191004]';

% hand measurements
%ft2mm = 12*25.4;
%cam(1).C = [ 1350 -1580 540]';
%cam(2).C = [ -450 770 (750+5*ft2mm) ]';
%cam(3).C = [ 1500 -1600 2250]';
%cam(4).C = [-280 820 570]';

% 2006 12 01 DLT in flydra WT
cam(1).C = [867.37446261  198.97798549  832.51824736]';
cam(2).C = [1194.97461953 206.718389208 898.66723201]';
cam(3).C = [459.669418616 168.068949056 713.890827]';
cam(4).C = [-186.0833207 207.892635698 775.298571621]';
cam(5).C = [115.230367611 202.584140956 904.723766629]';

% 2007 10 03 DLT in flydra WT
cam(1).C = [ 985.827871     182.49267738  1134.97921258]';
cam(2).C = [362.27817754   189.63204145  1134.23310277]';
cam(3).C = [721.10852967  167.31210242  932.32564817]';
cam(4).C = [-330.47764957   168.38518854  1014.81801646]';
cam(5).C = [1774.53765317   144.79725152   928.10954671]';

% of the similarity computation

[align.simT.s, align.simT.R, align.simT.t]  = estsimt([Cst'],[cam(:).C]);
[align.P, align.X] = align3d(in.Pe,in.Xe,align.simT);
% save aligned data
if 1 % SAVE_STEPHI | SAVE_PGUHA
	[align.Cst,align.Rot] = savecalpar(align.P,config);
end

if ~Octave,
  drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: Aligned data',config.cal.cams2use);

  set(gca,'CameraTarget',[0,0,0]);
  set(gca,'CameraPosition',[0,0,1]);

  figure(61),
  % print -depsc graphevalaligned.eps
  eval(['print -depsc ', config.paths.data, 'topview.eps'])

  drawscene(align.X,align.Cst',align.Rot,62,'cloud','Graphical Output Validation: Aligned data',config.cal.cams2use);

  set(gca,'CameraTarget',[0,0,0.9]);
  set(gca,'CameraPosition',[2,0,0.9]);

  %figure(62),
  % print -depsc graphevalaligned.eps
  %eval(['print -depsc ', config.paths.data, 'sideview.eps'])
end

return


function gX = lfmKernGradX(kern, x, x2)

% LFMKERNGRADX Gradient of LFM kernel with respect to a point x.
% FORMAT
% DESC computes the gradient of the single input motif
% kernel with respect to the input positions.
% ARG kern : kernel structure for which gradients are being
% computed.
% ARG x : locations against which gradients are being computed.
% RETURN g : the returned gradients. The gradients are returned in
% a matrix which is numData x numInputs x numData. Where numData is
% the number of data points and numInputs is the number of input
% dimensions in X.
%
% FORMAT
% DESC computes the gradident of the single input motif
% kernel with respect to the input positions where both the row
% positions and column positions are provided separately.
% ARG kern : kernel structure for which gradients are being
% computed.
% ARG x1 : row locations against which gradients are being computed.
% ARG x2 : column locations against which gradients are being computed.
% RETURN g : the returned gradients. The gradients are returned in
% a matrix which is numData2 x numInputs x numData1. Where numData1 is
% the number of data points in X1, numData2 is the number of data
% points in X2 and numInputs is the number of input
% dimensions in X.
%
% SEEALSO lfmKernParamInit, kernGradX, lfmKernDiagGradX
%
% COPYRIGHT : Neil D. Lawrence, 2006

% KERN

error('lfmKernGradX not yet implemented.')


function [pyra, model_dp] = gdetect_pos_prepare(im, model, boxes, fg_overlap)
% Prepare a set of foreground examples in the same image for processing
% with gdetect_pos.m.
%   [pyra, model_dp] = gdetect_pos_prepare(im, model, boxes, fg_overlap)
%
% Return values
%   pyra          Feature pyramid for image im
%   model_dp      Model augmented with dynamic programming tables
%
% Arguments
%   im            Foreground image with one or more foreground examples
%   model         Object model
%   boxes         Foreground example bounding boxes from foreground image im
%   fg_overlap    Amount of overlap required between a belief
%                 and a foreground example

% AUTORIGHTS
% -------------------------------------------------------
% Copyright (C) 2009-2012 Ross Girshick
%
% This file is part of the voc-releaseX code
% (http://people.cs.uchicago.edu/~rbg/latent/)
% and is available under the terms of an MIT-like license
% provided in COPYING. Please retain this notice and
% COPYING if you use this file (or a portion of it) in
% your project.
% -------------------------------------------------------

% get feature pyramid
pyra = featpyramid(im, model);

% mark valid levels (skip levels that don't have sufficient
% overlap with any box in boxes
pyra.valid_levels = validate_levels(model, pyra, boxes, fg_overlap);

% compute dynamic programming tables (stored in model_dp)
model_dp = gdetect_dp(pyra, model);

% compute overlap info for each component, box, and valid pyramid level
% (We end up computing overlap twice -- once here and once in
%  validate_levels. At the expense of making the code yet more complex
%  we could this computation only once. The reason this isn't straight-
%  forward is that the overlaps need to have exactly the same dimensions
%  as the score tables computed by gdetect_dp. But we don't want to call
%  gdetect_dp until we know which levels can be skipped, which requires
%  computing overlaps... At any rate, this isn't a major bottleneck.)
pyra.overlaps = compute_overlaps(pyra, model_dp, boxes);


function im = read_tiny_big_binary(words_in,ind)
%
%%% Routine to read in tiny images from giant binary file holding all images.
%
%%% There are two modes of operation:
%
% 1. raw indexing mode
% --------------------
% Inputs:
% 1. words_in - 1 x nImages vector, each element being an integer
% in the range 1 to 73,777,893 (or whatever the total number of images in the dataset is).
% Outputs:
% 1. im - 3072 x nImages uint8 data of images.
%
% ++++++++++++++++++++++++++++++++++++++++++++++++++++
%
% 2. word based indexing.
% -----------------------
% Inputs:
% 1. word_in - 1 x nWords cell array, each entry holding one noun
% 2. ind - 1 x nWords cell array, each holding indices of images
% for the corresponding noun
% Outputs:
% 1. im - 3072 x nImages uint8 data of images.
%
% Requires: read_tiny_binary_big_core.mex file
%
% Notes: (i) The code automatically sorts the indices so that they are in
%            order, to improve speed.
%
%       (ii) The access time per image drops as the total number of images to be
%            gathered increases. e.g. for 500,000 images load time is
%            1ms/image. For 100,000 images it is 2ms/image. So try and pass in big
%            blocks of indices.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SET PATHS BELOW TO tiny_images.bin AND tiny_index.mat

% path to giant binary file
binary_fname = '/mit/tiny/data/tiny_images.bin';
% indexing info .mat file
binary_data_fname = '/mit/tiny/data/tiny_index.mat';

% END
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% total # imgs
TOTAL_NUM_IMGS = 79302017;

if (nargin==1)
    RAW_INDEX_MODE = 1;
else
    RAW_INDEX_MODE = 0;
end

if (nargin==2)
    if iscell(ind)
        % do nothing
    else
        % convert to cell arrays
        words_in = {words_in};
        ind = {ind};
    end

    nWords = length(words_in);
end

if RAW_INDEX_MODE
    %% just go and call binary with raw indices
    %% check that all indices are >0 and <TOTAL_NUM_IMGS
    if ((sum(words_in<1)==0) & (sum(words_in>TOTAL_NUM_IMGS)==0))

        % now sort for speed
        [sorted_words,sort_ind] = sort(words_in);

        %% use MEX for sppedy reading in
        im = read_tiny_binary_big_core(binary_fname, uint64(sorted_words));

        % sort back
        im(:,sort_ind) = im;
    else
        error('indices <0 or greater than total number of images');
    end

else % words_in and indices passed in

    % load up index data
    load(binary_data_fname);

    % compute hashes for all words_in
    hashes = compute_hash_function(words_in,hash_key);

    img_offsets = [];

    for a=1:nWords

        % find word using hash
        match_ind = find(hashes(a) == hash);

        if any(match_ind)
            % check all ind are >=1 and <=total number of images

            if ((sum(ind{a}<1)==0) & (sum(ind{a}>num_imgs(match_ind))==0))
                word_offset = offset(match_ind);
                img_offsets = [img_offsets , uint64(double(word_offset) + [ind{a}])];
            else
                error('indices <0 or greater than number per image');
            end
        else
          %im = zeros(3072,1);
          %return;
          error('word not found');
        end

        % now sort for speed
        [sorted_offsets,sort_ind] = sort(img_offsets);

        im = read_tiny_binary_big_core(binary_fname, uint64(sorted_offsets));

        % sort back
        im(:,sort_ind) = im;
    end
end


function [ remove ] = selectHierarchies( data, NODETYPES )

%% select the hierarchies which meet our requirement

labellist = data.labellist;
kids = data.kids;
remove = 0;

% there should be one floor
ind = strmatch('floor',labellist,'exact');
if(length(ind)~=1)
    remove = 1;
    fprintf('don not have a floor\n');
end

% there should be one bed
%ind = strmatch('bed',labellist,'exact');
%if(length(ind)~=1)
%    remove = 1;
%    fprintf("don't have one bed\n");
%end

% the number of objects should be between 2~10, 8~15 including the walls
objnum = length(labellist);
%if(objnum<14||objnum>34)
if(objnum<8||objnum>15)
    remove = 1;
    fprintf('obj num:%d\n', objnum);
end

% check kids
for i = 1:length(kids)
    k = kids{i};
    if(length(k)>0)
        flag = k(1);
        if(flag~=0&&flag~=NODETYPES.COOCCUR&&flag~=NODETYPES.SUPPORT&&flag~=NODETYPES.SURROUND&&flag~=NODETYPES.ROOM&&flag~=NODETYPES.WALL)
            remove = 1;
            fprintf('node type error\n', objnum);
        end
        if(flag==NODETYPES.COOCCUR||flag==NODETYPES.SUPPORT||flag==NODETYPES.WALL)
            if(length(k)~=3)
                remove = 1;
                fprintf('group/support node error\n', objnum);
            end
        end
        if(flag==NODETYPES.SURROUND)
            if(length(k)~=4)
                remove = 1;
                fprintf('surround node error\n', objnum);
            end
        end
        if(flag==NODETYPES.ROOM)
            if(length(k)~=6)
                remove = 1;
                fprintf('room node error\n', objnum);
            end
        end
    else
        remove = 1;
    end
end

% set the max room size
MAXROOMSIZE = 7;
obblist = data.obblist;
for i = 1:size(obblist,2)
    cent = obblist(1:3,i);
    sizes = obblist(10:12,i);
    ind = find(abs(cent)>MAXROOMSIZE);
    ind2 = find(abs(sizes)>MAXROOMSIZE);
    if(length(ind)>0||length(ind2)>0)
        remove = 1;
        fprintf('exceed maxroom size\n', objnum);
    end
end

end


function varargout = interp1_internal(varargin)
%INTERP1_INTERNAL (overloaded)

switch class(varargin{1})

    case 'double'
        if isequal(varargin{4},'graph') || isequal(varargin{4},'lp') || isequal(varargin{4},'milp') || isequal(varargin{4},'sos2')
            varargout{1} = interp1(varargin{2},varargin{3},varargin{1},'linear');
        else
            varargout{1} = interp1(varargin{2},varargin{3},varargin{1},varargin{4});
        end

    case 'char'

        t = varargin{2};
        x = varargin{3};
        xi = varargin{4};
        yi = varargin{5};
        method = varargin{6};
        dyi = varargin{7};

        if strcmpi(varargin{1},'graph')
            % Convexity propagation wants epi-graph models, so is that what
            % we have used for modelling?
            if strcmpi(method,'envelope')
                   [xi,yi] = convhullprune(xi,yi);
                   [Model,mono,def] = convexGraph(xi,yi,x,t,dyi);
                   operator = struct('convexity','convex','monotonicity',mono,'definiteness',def,'model','graph');
            elseif strcmpi(method,'graph') || strcmpi(method,'lp')
                if isconvexdata(xi,yi)
                    % Convex case, create epi-graph
                    [Model,mono,def] = convexGraph(xi,yi,x,t,dyi);
                    operator = struct('convexity','convex','monotonicity',mono,'definiteness',def,'model','graph');
                elseif isconvexdata(xi,-yi)
                    [Model,mono,def] = concaveGraph(xi,yi,x,t,dyi);
                    operator = struct('convexity','concave','monotonicity',mono,'definiteness',def,'model','graph');
                else
                    Model = [];
                    operator = [];
                end
            else
                Model = [];
                operator = [];
            end
        else
            if strcmpi(method,'sos2') || strcmpi(method,'milp') || strcmpi(method,'lp')
                lambda = sdpvar(length(xi),1);
                Model = [sos2(lambda), x == lambda'*xi(:), t == lambda'*yi(:),lambda>=0, sum(lambda)==1];
                [mono,def] = classifyData(yi);
                operator = struct('convexity','none','monotonicity',mono,'definiteness',def,'model','integer');
            elseif strcmpi(method,'graph')
                Model = [];
                operator = [];
            else
                Model = [min(xi) <= varargin{3} <= max(xi)];
                operator = struct('convexity','none','monotonicity','none','definiteness','none','model','callback');
                operator.bounds = @bounds;
            end
        end

        varargout{1} = Model;
        varargout{2} = operator;
        varargout{3} = varargin{3};

    otherwise
        error('SDPVAR/INTERP1_INTERNAL called with strange argument');
end

function [L,U] = bounds(xL,xU,varargin)

xv = varargin{1};
yv = varargin{2};
if xL <= xv(1)
    index1 = 1;
else
    index1 = find(xL > xv);index1 = max(1,max(index1));
end
if xU >= xv(end)
    index2 = length(xv);
else
    index2 = find(xv < xU);index2 = min(length(xv),max(index2)+1);
end

if isequal(varargin{3},'linear')
    Z = yv(index1:index2);
    L = min(yv(index1:index2));
    U = max(yv(index1:index2));
else
    N = ceil((xv(index2)-xv(index1))/(mean(diff(xv))/100));
    z = linspace(xv(index1),xv(index2),N);
    yz = interp1(xv,yv,z,varargin{3});
    % To account for finite grid, we add a precision dependent margin
    dz = (z(2)-z(1));
    L = min(yz)-dz;
    U = max(yz)+dz;
end

function isconvex = isconvexdata(xi,yi)
finterp = (yi(1:end-2) + yi(3:end))/2;
if all(finterp >= yi(2:end-1))
    isconvex = 1;
else
    isconvex = 0;
end

function [Model,mono,def] = convexGraph(xi,yi,x,t,dyi)

Model = [min(xi) <= x <= max(xi)];
if isempty(dyi)
    grads = diff(yi)./diff(xi);
    Model = [Model, yi(1:end-1) + grads.*(x - xi(1:end-1)) <= t];
else
    grads = dyi;
    Model = [Model, yi(:) + grads(:).*(x - xi(:)) <= t];
end
[mono,def] = classifyData(yi);

function [Model,mono,def] = concaveGraph(xi,yi,x,t,dyi)

Model = [min(xi) <= x <= max(xi)];
if isempty(dyi)
    grads = diff(yi)./diff(xi);
    Model = [Model, yi(1:end-1) + grads.*(x - xi(1:end-1)) >= t];
else
    grads = dyi;
    Model = [Model, yi(:) + grads(:).*(x - xi(:)) >= t];
end

[mono,def] = classifyData(yi);

function [xout,yout] = convhullprune(xin,yin)
% Prunes data down to the lower convex envelope

% First prune by convex hull
cj = convhulln([xin(:) yin(:)]);
cj = sort(unique(cj));
x = xin(cj);
y = yin(cj);

% This can probably be done in linear complexity, I am just lazy here
keep = ones(length(x),1);
for i = 1:length(x)-1
    if keep(i)
        for j = length(x):-1:3
            if keep(j)
                grad = (y(j)-y(i))/(x(j)-x(i));
                k = i+1:j-1;
                fail =  y(k) > y(i) + grad*(x(k)-x(i));
                keep(k(fail)) = 0;
            end
        end
    end
end
keep = cj(find(keep));
xout = xin(keep);
yout = yin(keep);

function [mono,def] = classifyData(yi);
if all(diff(yi)) >= 0
    mono = 'increasing';
elseif all(diff(yi) <= 0)
    mono = 'decreasing';
else
    mono = 'none';
end
if all(yi>=0)
    def = 'positive';
elseif all(yi <= 0)
    def = 'negative';
else
    def = 0;
end


function bsdsAnalyzeResult(pb, imfn, outdir)

for f = 1:numel(pb)
    tmp = load(['~/data/eccv08/bsds/pb/' strtok(imfn{f}, '.') '_pb.mat']);
    pb{f} = bsdsNonMaxSuppression(pb{f}, tmp.pb);
    imwrite(pb{f}, [outdir strtok(imfn{f}, '.') '.bmp']);
end

boundaryBench(outdir,'color', 25, 1)


function obj = addJumpConstraint(obj, edge, src, tar, bounds, varargin)
    % Add jump constraints between the edge and neighboring two nodes
    % (source and target)
    %
    % These constraints include the continuity of the states/time
    % variables, as well as system-specific discrete map constraint of the
    % guard dynamics, and user-specific constraints.
    %
    % Parameters:
    % src: the source node NLP @type TrajectoryOptimization
    % edge: the edge NLP @type TrajectoryOptimization
    % tar: the target node NLP @type TrajectoryOptimization
    % bounds: the boundary values @type struct
    % varargin: extra argument @type varargin

    %% continuity of time
    t_s = SymVariable('ts',[2,1]);
    t_n = SymVariable('tn',[2,1]);
    t_cont = SymFunction('tContDomain',flatten(t_s(2)-t_n(1)),{t_s,t_n});

    % create a NlpFunction for 'edge' NLP, but use the NlpVariables from
    % 'src' and 'tar' NLPs.
    if src.Options.DistributeTimeVariable
        src_time_node = src.NumNode;
    else
        src_time_node = 1;
    end
    t_cstr = NlpFunction('Name','tContDomain',...
        'Dimension',1,...
        'lb', 0,...
        'ub', 0,...
        'Type','Linear',...
        'SymFun',t_cont,...
        'DepVariables',[src.OptVarTable.T(src_time_node);tar.OptVarTable.T(1)]);
    edge.addConstraint('tContDomain','first',t_cstr);

    %% state continuity (src <-> edge)
    x_s = src.Plant.States.x;
    x_e = SymVariable('xp',size(x_s));
    x_cont_src = SymFunction(['xMinusCont_' edge.Name],x_s-x_e,{x_s,x_e});
    x_src_cstr = NlpFunction('Name',['xMinusCont_' edge.Name],...
        'Dimension',src.Plant.numState,...
        'lb', 0,...
        'ub', 0,...
        'Type','Linear',...
        'SymFun',x_cont_src,...
        'DepVariables',[src.OptVarTable.x(end);edge.OptVarTable.x(1)]);
    edge.addConstraint('xMinusCont','first',x_src_cstr);

    %% state continuity (edge <-> tar)
    x_t = tar.Plant.States.x;
    x_e = edge.Plant.States.xn;
    x_cont_tar = SymFunction(['xPlusCont_' edge.Name],x_e-x_t,{x_e,x_t});
    x_tar_cstr = NlpFunction('Name',['xPlusCont_' edge.Name],...
        'Dimension',tar.Plant.numState,...
        'lb', 0,...
        'ub', 0,...
        'Type','Linear',...
        'SymFun',x_cont_tar,...
        'DepVariables',[edge.OptVarTable.xn(1);tar.OptVarTable.x(1)]);
    edge.addConstraint('xPlusCont','first',x_tar_cstr);

    if strcmp(edge.Plant.Type,'SecondOrder')
        %% state derivative continuity (src <-> edge)
        dx_s = src.Plant.States.dx;
        dx_e = SymVariable('xp',size(x_s));
        dx_cont_src = SymFunction(['dxMinusCont_' edge.Name],dx_s-dx_e,{dx_s,dx_e});
        dx_src_cstr = NlpFunction('Name',['dxMinusCont_' edge.Name],...
            'Dimension',src.Plant.numState,...
            'lb', 0,...
            'ub', 0,...
            'Type','Linear',...
            'SymFun',dx_cont_src,...
            'DepVariables',[src.OptVarTable.dx(end);edge.OptVarTable.dx(1)]);
        edge.addConstraint('dxMinusCont','first',dx_src_cstr);

        %% state derivative continuity (edge <-> tar)
        dx_t = tar.Plant.States.dx;
        dx_e = edge.Plant.States.dxn;
        dx_cont_tar = SymFunction(['dxPlusCont_' edge.Name],dx_e-dx_t,{dx_e,dx_t});
        dx_tar_cstr = NlpFunction('Name',['dxPlusCont_' edge.Name],...
            'Dimension',tar.Plant.numState,...
            'lb', 0,...
            'ub', 0,...
            'Type','Linear',...
            'SymFun',dx_cont_tar,...
            'DepVariables',[edge.OptVarTable.dxn(1);tar.OptVarTable.dx(1)]);
        edge.addConstraint('dxPlusCont','first',dx_tar_cstr);
    end

    %% the event function constraint for the source domain
    event_list = fieldnames(src.Plant.EventFuncs);  % all events
    if ~isempty(event_list)
        % find the index of the event associated with the edge
        event_idx = str_index(edge.Plant.EventName,event_list);
        % extract the event function object using the index
        event_obj = src.Plant.EventFuncs.(event_list{event_idx});
        % impose the NLP constraints (unilateral constraints)
        event_obj.imposeNLPConstraint(src);
        % update the upper bound at the last node to be zero (to ensure equality)
        event_cstr_name = event_obj.ConstrExpr.Name;
        updateConstrProp(src,event_cstr_name,'last','ub',0);
    end

    %% call the system constraint callback method for the discrete dyamics
    plant = edge.Plant;
    plant.UserNlpConstraint(edge, src, tar, bounds, varargin{:});
end


classdef Tests < matlab.unittest.TestCase
    % SolverTest tests solutions to the quadratic equation
    % a * x^2 + b * x + c = 0

    methods (Test)
        function testVehicleSimple(testCase)
            % Simulation time
            T = 6;                      % Total simulation time [s]
            resol = 50;                 % Resolution
            TSPAN = 0:T/resol:T;        % Time span [s]

            %% Tire parameters
            % Chosen tire: <TirePacejka.html TirePacejka.m>.
            %

            TireModel = VehicleDynamicsLateral.TirePacejka();

            %% Vehicle parameters
            % Chosen Vehicle: <VehicleSimpleNonlinear.html VehicleSimpleNonlinear.m>.

            System = VehicleDynamicsLateral.VehicleSimpleNonlinear();
            System.tire = TireModel;

            % Initial conditions
            System.dPSI0 = 0.7;                % Initial yaw rate [rad/s]
            System.ALPHAT0 = -0.2;             % Initial side slip angle [rad]
            System.PSI0 = 0;                   % Initial yaw angle [rad]
            System.X0 = 0;                     % Initial CG horizontal position [m]
            System.Y0 = 0;                     % Initial CG vertical position [m]
            System.V0 = 20;                    % Initial CG velocity [m/s]

            simulator = VehicleDynamicsLateral.Simulator(System, TSPAN);
            simulator.Simulate();

            % Retrieving states
            dPSI = simulator.dPSI;
            ALPHAT = simulator.ALPHAT;
            PSI = simulator.PSI;
            XT = simulator.XT;
            YT = simulator.YT;
            VEL = simulator.VEL;
        end
    end
end

%% See Also
%
% <../../index.html Home>
%


function selected_points = test_terrain()
% NOTEST

import iris.terrain_grid.*;
import iris.inflate_region;
import iris.thirdParty.polytopes.*;
import iris.cspace.*;
import iris.drawing.*;
lcmgl = drake.util.BotLCMGLClient(lcm.lcm.LCM.getSingleton(), 'segmentation');
% load('example_feas_map.mat','Q');
load('data/example_heights.mat','heights','px2world');
load('data/example_state.mat', 'x0');

px2world(1,end) = px2world(1,end) - sum(px2world(1,1:3)); % stupid matlab 1-indexing...
px2world(2,end) = px2world(2,end) - sum(px2world(2,1:3));
resample = 2;
mag = 2^(resample-1);
heights = interp2(heights, (resample-1));
px2world = px2world * [1/mag 0 0 (1-1/mag); 0 1/mag 0 (1-1/mag ); 0 0 1 0; 0 0 0 1];

world2px = inv(px2world);
world2px_2x3 = world2px(1:2,[1,2,4]);
px2world_2x3 = px2world(1:2, [1,2,4]);

Q = imfilter(heights, [1, -1]) - (0.06/mag) > 0;
Q = Q | imfilter(heights, [-1, 1]) - (0.06/mag) > 0;
Q = Q | imfilter(heights, [1; -1]) - (0.06/mag) > 0;
Q = Q | imfilter(heights, [-1; 1]) - (0.06/mag) > 0;
Q(isnan(heights)) = 1;

% grid = Q(85:125,25:85);
grid = ~Q;
lb = [180;70;-pi];
ub = [245; 160; pi];
A_bounds = [-1,0,0;
            0,-1,0;
            0,0,-1;
            1,0,0;
            0,1,0;
            0,0,1];
b_bounds = [-lb;ub];

bot = world2px_2x3 * 0.9*bsxfun(@minus, [-0.0820,-0.0820, 0.1780, 0.1780;
                                         0.0624,-0.0624, 0.0624,-0.0624;
                                         0,0,0,0], [0.0480; 0; 0;]);
% obstacles = segment_grid(~grid(lb(1):ub(1),lb(2):ub(2)));
% for j = 1:length(obstacles)
%   obstacles{j} = bsxfun(@plus, obstacles{j}, lb(1:2)-1);
% end
% obstacles = cspace3(obstacles, bot, 4);
selected_points = [];

while true
  figure(6)
  imshow(grid, 'InitialMagnification', 'fit')
  [c,r] = ginput(1);
  if ~isempty(c)
    selected_points(:,end+1) = [r;c];
  end
  if isempty(c)
    break
  end
  c = round(c);
  r = round(r);
  black_edges = [];
  [black_edges(1,:), black_edges(2,:)] = ind2sub(size(grid), find(component_boundary(grid, [r;c])));
  obs_mask = all(bsxfun(@minus, A_bounds([1,2,4,5],1:2) * black_edges, b_bounds([1,2,4,5])) <= max(max(bot)));
  obstacles = mat2cell(black_edges(:,obs_mask) , 2, ones(1,sum(obs_mask)));
  obstacles = cspace3(obstacles, bot, 4);

  [A,b,C,d,results] = inflate_region(obstacles, A_bounds, b_bounds, [r;c;0]);
%   animate_results(results);

  [inner_poly, outer_poly] = project_c_space_region(A,b);

  % px_xy = [V(:,2)'; V(:,1)'];
  if ~isempty(inner_poly)
    px_xy = [inner_poly(2,:); inner_poly(1,:)];
    world_xy = px2world_2x3 * [px_xy; ones(1,size(px_xy,2))];
    z = ones(size(px_xy,2)) * heights(r,c) + 0.03;
    world_xyz = [world_xy; z];
    lcmgl.glColor3f(0,1,0);
    lcmgl.glLineWidth(10);
    lcmgl.glBegin(lcmgl.LCMGL_LINES);
    for j = 1:size(world_xy,2)-1
      lcmgl.glVertex3d(world_xyz(1,j),world_xyz(2,j),world_xyz(3,j));
      lcmgl.glVertex3d(world_xyz(1,j+1),world_xyz(2,j+1),world_xyz(3,j+1));
    end
    lcmgl.glVertex3d(world_xyz(1,end),world_xyz(2,end),world_xyz(3,end));
    lcmgl.glVertex3d(world_xyz(1,1),world_xyz(2,1),world_xyz(3,1));
    lcmgl.glEnd();
  end

  px_xy = [outer_poly(2,:); outer_poly(1,:)];
  world_xy = px2world_2x3 * [px_xy; ones(1,size(px_xy,2))];
  z = ones(size(px_xy,2)) * heights(r,c) + 0.03;
  world_xyz = [world_xy; z];
  lcmgl.glColor3f(1,1,0);
  lcmgl.glLineWidth(10);
  lcmgl.glBegin(lcmgl.LCMGL_LINES);
  for j = 1:size(world_xy,2)-1
    lcmgl.glVertex3d(world_xyz(1,j),world_xyz(2,j),world_xyz(3,j));
    lcmgl.glVertex3d(world_xyz(1,j+1),world_xyz(2,j+1),world_xyz(3,j+1));
  end
  lcmgl.glVertex3d(world_xyz(1,end),world_xyz(2,end),world_xyz(3,end));
  lcmgl.glVertex3d(world_xyz(1,1),world_xyz(2,1),world_xyz(3,1));
  lcmgl.glEnd();

  lcmgl.glColor3f(0,1,0);
  lcmgl.sphere([(px2world_2x3 * [c;r;1])', heights(r,c)], 0.05, 20, 20);

%   lcmgl.switchBuffers();
%   plot_lcm_points([px_xy', px_z'],repmat([1,0,0],length(px_z),1),101,'region1',3,1);
end
lcmgl.switchBuffers();
end


function [dtdx,dtrp,vart,stat]=model_trop(time,pos,azel,rtk,x,nav) %#ok

global glc;
stat=0;
trp=zeros(3,1); dtdx=zeros(3,1); ERR_SAAS=0.3;

if rtk.opt.tropopt==glc.TROPOPT_SAAS
    dtrp=trop_saa(pos,azel,0.7); %humi=0.7
    vart=ERR_SAAS^2;
    stat=1; return;
end

if rtk.opt.tropopt==glc.TROPOPT_EST||rtk.opt.tropopt==glc.TROPOPT_ESTG
    if rtk.opt.tropopt==glc.TROPOPT_EST,trp(1)=x(rtk.it+1);
    else,trp(1:3)=x(rtk.it+1:rtk.it+3);
    end
    [dtdx,dtrp,vart]=trop_model_prec(time,pos,azel,trp);
    stat=1;return;
end

return


%DEMO_MEMORYSAVE  Demonstration of memory save option in GPstuff
%
%  Description
%    This demo consists of various combinations of covariance functions,
%    likelihoods and full/sparse approximations. This demo is intended for
%    showing how to use memory save option in GPstuff. Unfortunately MATLAB
%    doesn't have memory usage monitoring, so the purpose of the demo is only
%    to test that results are the same whether you use memory save or not
%    and to show that the running times with memory saving are little bit longer
%    (more overhead). Memory saving can be enabled with the following command
%
%       gp = gp_set(..., 'savememory', 'on');
%
%  See also
%    GP_SET
%

% Copyright (c) 2012 Ville Tolvanen

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

% Classification data with full GP's

S = which('demo_classific');
L = strrep(S,'demo_classific.m','demodata/synth.tr');
x=load(L);
y=x(:,end);
y = 2.*y-1;
x(:,end)=[];

lik = lik_logit();

pl = prior_t();
pm = prior_sqrtunif();

gpcf = gpcf_neuralnetwork('weightSigma2', [0.9 0.9], 'biasSigma2', 10);
gpcf = gpcf_neuralnetwork(gpcf, 'weightSigma2_prior', pl,'biasSigma2_prior', pm); %

gpcf2 = gpcf_sexp('lengthScale', [0.9 0.9], 'magnSigma2', 10);
gpcf2 = gpcf_sexp(gpcf2, 'lengthScale_prior', pl,'magnSigma2_prior', pm); %

% GP without memory saving
gp = gp_set('lik', lik, 'cf', gpcf, 'jitterSigma2', 1e-4, 'latent_method', 'EP');
% GP with memory saving option enabled
gp2 = gp_set('lik', lik, 'cf', gpcf, 'jitterSigma2', 1e-4, 'latent_method', 'EP', 'savememory', 'on');

fprintf('Classification (EP), Neuralnetwork covariance function without and with memory saving (optimization and prediction)\n');
opt=optimset('TolX',1e-3,'TolFun',1e-3, 'Display', 'off');

% Optimization and prediction without memory saving
tic,gp=gp_optim(gp,x,y,'opt',opt);
[Eft,Varft, lpyt]=gp_pred(gp,x,y,x, 'yt', y);toc

% Optimization and prediction with memory saving
tic,gp2=gp_optim(gp2,x,y,'opt',opt);
[Eft2,Varft2, lpyt2]=gp_pred(gp2,x,y,x, 'yt', y);toc

% Check that predictions (and optimization) with and without memory saving are same
assert(all(Eft==Eft2)); assert(all(Varft==Varft2)); assert(all(lpyt==lpyt2));

gp = gp_set('lik', lik, 'cf', gpcf2, 'jitterSigma2', 1e-6, 'latent_method', 'Laplace');
gp2 = gp_set('lik', lik, 'cf', gpcf2, 'jitterSigma2', 1e-6, 'latent_method', 'Laplace', 'savememory', 'on');

fprintf('Classification (Laplace), Squared-Exponential covariance function without and with memory saving (optimization and prediction)\n');

% Optimization and prediction without memory saving
tic,gp=gp_optim(gp,x,y,'opt',opt);
[Eft,Varft, lpyt]=gp_pred(gp,x,y,x, 'yt', y);toc

% Optimization and prediction with memory saving
tic,gp2=gp_optim(gp2,x,y,'opt',opt);
[Eft2,Varft2, lpyt2]=gp_pred(gp2,x,y,x, 'yt', y);toc

% Check that predictions (and optimization) with and without memory saving are same
assert(all(Eft==Eft2)); assert(all(Varft==Varft2)); assert(all(lpyt==lpyt2));

% Regression data with sparse approximations

prevstream=setrandstream();
xx=linspace(1,10,901);
x1=logspace(0,1,100);
x1=round(x1*100)-99;
x=xx(x1)';
y=2*sin(4*x)+0.2*randn(size(x));
xt=[1:0.01:14]';
[n,nin] = size(x);

pl = prior_t('s2', 1);
pm = prior_logunif();
pn = prior_logunif();
gpcfse = gpcf_matern32('lengthScale',0.5,'magnSigma2',1, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);
lik = lik_gaussian('sigma2', 0.1, 'sigma2_prior', pn);
gp = gp_set('lik', lik, 'cf', gpcfse, 'jitterSigma2', 1e-6);
opt=optimset('TolFun',1e-4,'TolX',1e-4);

fprintf('Regression, FIC GP, Matern-3/2 covariance function, without and with memory saving (optimization and prediction)\n')
Xu=round(10+90*rand(18,1))/10; % Random placement

gp_fic = gp_set(gp, 'type','FIC','X_u',Xu,'infer_params','covariance+likelihood+inducing');
gp_fic2 = gp_set(gp, 'type','FIC','X_u',Xu,'infer_params','covariance+likelihood+inducing', 'savememory', 'on');

opt=optimset('TolFun',1e-4,'TolX',1e-4, 'Display', 'off');

% Optimization and prediction without memory saving
tic,gp_fic=gp_optim(gp_fic,x,y,'opt',opt);
[Eft_fic, Varft_fic] = gp_pred(gp_fic, x, y, xt);
Varft_fic = Varft_fic + gp_fic.lik.sigma2;toc

% Optimization and prediction with memory saving
tic,gp_fic2=gp_optim(gp_fic2,x,y,'opt',opt);
[Eft_fic2, Varft_fic2] = gp_pred(gp_fic2, x, y, xt);
Varft_fic2 = Varft_fic2 + gp_fic.lik.sigma2;toc
setrandstream([],prevstream);

% Check that predictions (and optimization) with and without memory saving are same
assert(all(Eft_fic==Eft_fic2)); assert(all(Varft_fic==Varft_fic2));

gpcfse = gpcf_sexp('lengthScale',0.5,'magnSigma2',1, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);
gp = gp_set('lik', lik, 'cf', gpcfse, 'jitterSigma2', 1e-6);

gp_fic = gp_set(gp, 'type','VAR','X_u',Xu,'infer_params','covariance+likelihood+inducing');
gp_fic2 = gp_set(gp, 'type','VAR','X_u',Xu,'infer_params','covariance+likelihood+inducing', 'savememory', 'on');

fprintf('Regression, VAR GP, Squared-Exponential covariance function, without and with memory saving (optimization and prediction)\n')

% Optimization and prediction without memory saving
tic,gp_fic=gp_optim(gp_fic,x,y,'opt',opt);
[Eft_fic, Varft_fic] = gp_pred(gp_fic, x, y, xt);
Varft_fic = Varft_fic + gp_fic.lik.sigma2;toc

% Optimization and prediction with memory saving
tic,gp_fic2=gp_optim(gp_fic2,x,y,'opt',opt);
[Eft_fic2, Varft_fic2] = gp_pred(gp_fic2, x, y, xt);
Varft_fic2 = Varft_fic2 + gp_fic.lik.sigma2;toc
setrandstream([],prevstream);

% Check that predictions (and optimization) with and without memory saving are same
assert(all(Eft_fic==Eft_fic2)); assert(all(Varft_fic==Varft_fic2));

% Spatial data with sparse approximations

S = which('demo_spatial1');
data = load(strrep(S,'demo_spatial1.m','demodata/spatial1.txt'));
x = data(1:200,1:2);
ye = data(1:200,3);
y = data(1:200,4);
dims = [1    60     1    35];
[trindex, Xu] = set_PIC(x, dims, 5, 'corners', 0);
[n,nin] = size(x);

pl = prior_t('s2',10);
pm = prior_sqrtunif();

gpcf1 = gpcf_ppcs3('nin',nin,'lengthScale', 1, 'magnSigma2', 0.1, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);
gpcf2 = gpcf_sexp('lengthScale', 5, 'magnSigma2', 0.05, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);
gpcf3 = gpcf_neuralnetwork('weightSigma2', [1 1], 'biasSigma2', 0.05, 'weightSigma2_prior', pl, 'biasSigma2_prior', pm);

lik = lik_negbin();
% GP without memory save
gp = gp_set('type', 'FIC', 'lik', lik, 'cf', gpcf1, 'X_u', Xu, ...
            'jitterSigma2', 1e-4, 'infer_params', 'covariance+inducing');
% GP with memory saving option enabled
gp2 = gp_set('type', 'FIC', 'lik', lik, 'cf', gpcf1, 'X_u', Xu, ...
            'jitterSigma2', 1e-4, 'infer_params', 'covariance+inducing', 'savememory', 'on');
opt=optimset('TolFun',1e-2,'TolX',1e-2, 'Display', 'off');

fprintf('Spatial process (Laplace), FIC GP, PPCS3 covariance function, without and with memory saving (optimization and prediction)\n');

% Optimization and prediction without memory saving
tic,gp=gp_optim(gp,x,y,'z',ye,'opt',opt);
[Ef, Varf] = gp_pred(gp, x, y, x, 'z', ye, 'tstind', [1:n]); toc

% Optimization and prediction with memory saving
tic,gp2=gp_optim(gp2,x,y,'z',ye,'opt',opt);
[Ef2, Varf2] = gp_pred(gp2, x, y, x, 'z', ye, 'tstind', [1:n]); toc

% Check that predictions (and optimization) with and without memory saving are same
assert(all(Ef==Ef2)); assert(all(Varf==Varf2));

gp = gp_set('lik', lik, 'cf', gpcf2, 'jitterSigma2', 1e-4, 'latent_method', 'EP');
gp2 = gp_set('lik', lik, 'cf', gpcf2, 'jitterSigma2', 1e-4, 'latent_method', 'EP', 'savememory', 'on');

fprintf('Spatial process (EP), FIC GP, Squared-Exponential covariance function, without and with memory saving (optimization and prediction)\n');

% Optimization and prediction without memory saving
tic,gp=gp_optim(gp,x,y,'z',ye,'opt',opt);
[Ef, Varf] = gp_pred(gp, x, y, x, 'z', ye, 'tstind', [1:n]); toc

% Optimization and prediction with memory saving
tic,gp2=gp_optim(gp2,x,y,'z',ye,'opt',opt);
[Ef2, Varf2] = gp_pred(gp2, x, y, x, 'z', ye, 'tstind', [1:n]); toc

% Check that predictions (and optimization) with and without memory saving are same
assert(all(Ef==Ef2)); assert(all(Varf==Varf2));

gp = gp_set('lik', lik, 'cf', gpcf3, 'jitterSigma2', 1e-4);
gp2 = gp_set('lik', lik, 'cf', gpcf3, 'jitterSigma2', 1e-4, 'savememory', 'on');

fprintf('Spatial process (Laplace), FIC GP, Neuralnetwork covariance function, without and with memory saving (optimization and prediction)\n');
tic,gp=gp_optim(gp,x,y,'z',ye,'opt',opt);
[Ef, Varf] = gp_pred(gp, x, y, x, 'z', ye, 'tstind', [1:n]); toc

tic,gp2=gp_optim(gp2,x,y,'z',ye,'opt',opt);
[Ef2, Varf2] = gp_pred(gp2, x, y, x, 'z', ye, 'tstind', [1:n]); toc

% Check that predictions (and optimization) with and without memory saving are same
assert(all(Ef==Ef2)); assert(all(Varf==Varf2));


function [ out ] = func_projection_filterbank( dat, w )

CSP_W=w;

for ii=1:length(CSP_W)
    SMT{ii}=func_projection(dat{ii}, CSP_W{ii});
end

out=SMT;
end


function test_bug2375

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_prepare_headmodel ft_headmodel_localspheres

load(dccnpath('/home/common/matlab/fieldtrip/data/test/bug2375/localspheres_bug.mat'));

vol = ft_prepare_headmodel(cfg, headshape);

% they should all have the same sphere, since the cfg is faulty
assert(all(vol.r==vol.r(1)));

% fix the cfg and try again
cfg = rmfield(cfg, 'radius');
cfg = rmfield(cfg, 'maxradius');
vol = ft_prepare_headmodel(cfg, headshape);

% they should not all have the same sphere any more
assert(~all(vol.r==vol.r(1)));


% Script to compile C code for EEG-BEM

% Christophe Phillips
% $Id: make.m 2718 2009-02-09 19:14:20Z vladimir $

mex -O bem_Cii_cog.c
mex -O bem_Cii_cst.c
mex -O bem_Cii_lin.c
mex -O bem_Cij_cog.c
mex -O bem_Cij_cst.c
mex -O bem_Cij_lin.c
mex -O bem_Gi_cog.c
mex -O bem_Gi_vert.c