% Fifteen Game for MATLAB
% (c) 2010 Mihir Shah

function varargout = Fifteen(varargin)
% FIFTEEN M-file for Fifteen.fig
%      FIFTEEN, by itself, creates a new FIFTEEN or raises the existing
%      singleton*.
%
%      H = FIFTEEN returns the handle to a new FIFTEEN or the handle to
%      the existing singleton*.
%
%      FIFTEEN('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in FIFTEEN.M with the given input arguments.
%
%      FIFTEEN('Property','Value',...) creates a new FIFTEEN or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before Fifteen_OpeningFcn gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to Fifteen_OpeningFcn via varargin.
%
%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
%      instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Edit the above text to modify the response to help Fifteen

% Last Modified by GUIDE v2.5 06-Jul-2010 16:20:52

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @Fifteen_OpeningFcn, ...
                   'gui_OutputFcn',  @Fifteen_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT

% --- Executes just before Fifteen is made visible.
function Fifteen_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to Fifteen (see VARARGIN)

% Choose default command line output for Fifteen
handles.output = hObject;
guidata(hObject, handles);
clc;
clear all;
global x;
x = zeros(4);
i=1;
while i<16 && i>0
    m = randi([1,4]);
    n = randi([1,4]);
    if x(m,n) == 0
        x(m,n) = i;
        i = i + 1;
    end
end
display(x);
global counter;
counter = 0;
tic
% Update handles structure

% --- Outputs from this function are returned to the command line.
function varargout = Fifteen_OutputFcn(hObject, eventdata, handles)
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
set(handles.p_counter,'String','0');
set(handles.p_clock,'String','0');
varargout{1} = handles.output;

function a = access()
global x;
a = x;

% --- Executes on button press in p11.
function p11_Callback(hObject, eventdata, handles)
% hObject    handle to p11 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

global p;
p = 1;
p_setter_Callback(hObject, eventdata, handles)
p_slide_Callback(hObject, eventdata, handles)
p_win_Callback(hObject, eventdata, handles)
% if ~bcolor(1,2)
%     stra = get(handles.p11,'String');
%     strb = get(handles.p12,'String');
%     temp = stra;
%     stra = strb;
%     strb = temp;
%     set(handles.p11,'String',stra);
%     set(handles.p12,'String',strb);
%     set(handles.p11,'BackgroundColor','black');
%     bcolor(1,1) = 0;
%     bcolor(1,2) = 1;
%     set(handles.p12,'BackgroundColor','white');
% end
guidata(hObject, handles);

% --- Executes on button press in p12.
function p12_Callback(hObject, eventdata, handles)
global p;
p = 2;
p_setter_Callback(hObject, eventdata, handles)
p_slide_Callback(hObject, eventdata, handles)
p_win_Callback(hObject, eventdata, handles)
guidata(hObject, handles);

% --- Executes on button press in p13.
function p13_Callback(hObject, eventdata, handles)
global p;
p = 3;
p_setter_Callback(hObject, eventdata, handles)
p_slide_Callback(hObject, eventdata, handles)
p_win_Callback(hObject, eventdata, handles)
guidata(hObject, handles);

% --- Executes on button press in p14.
function p14_Callback(hObject, eventdata, handles)
global p;
p = 4;
p_setter_Callback(hObject, eventdata, handles)
p_slide_Callback(hObject, eventdata, handles)
p_win_Callback(hObject, eventdata, handles)
guidata(hObject, handles);

% --- Executes on button press in p21.
function p21_Callback(hObject, eventdata, handles)
global p;
p = 5;
p_setter_Callback(hObject, eventdata, handles)
p_slide_Callback(hObject, eventdata, handles)
p_win_Callback(hObject, eventdata, handles)
guidata(hObject, handles);

% --- Executes on button press in p22.
function p22_Callback(hObject, eventdata, handles)
global p;
p = 6;
p_setter_Callback(hObject, eventdata, handles)
p_slide_Callback(hObject, eventdata, handles)
p_win_Callback(hObject, eventdata, handles)
guidata(hObject, handles);

% --- Executes on button press in p23.
function p23_Callback(hObject, eventdata, handles)
global p;
p = 7;
p_setter_Callback(hObject, eventdata, handles)
p_slide_Callback(hObject, eventdata, handles)
p_win_Callback(hObject, eventdata, handles)
guidata(hObject, handles);

% --- Executes on button press in p24.
function p24_Callback(hObject, eventdata, handles)
global p;
p = 8;
p_setter_Callback(hObject, eventdata, handles)
p_slide_Callback(hObject, eventdata, handles)
p_win_Callback(hObject, eventdata, handles)
guidata(hObject, handles);

% --- Executes on button press in p31.
function p31_Callback(hObject, eventdata, handles)
global p;
p = 9;
p_setter_Callback(hObject, eventdata, handles)
p_slide_Callback(hObject, eventdata, handles)
p_win_Callback(hObject, eventdata, handles)
guidata(hObject, handles);

% --- Executes on button press in p32.
function p32_Callback(hObject, eventdata, handles)
global p;
p = 10;
p_setter_Callback(hObject, eventdata, handles)
p_slide_Callback(hObject, eventdata, handles)
p_win_Callback(hObject, eventdata, handles)
guidata(hObject, handles);

% --- Executes on button press in p33.
function p33_Callback(hObject, eventdata, handles)
global p;
p = 11;
p_setter_Callback(hObject, eventdata, handles)
p_slide_Callback(hObject, eventdata, handles)
p_win_Callback(hObject, eventdata, handles)
guidata(hObject, handles);

% --- Executes on button press in p34.
function p34_Callback(hObject, eventdata, handles)
global p;
p = 12;
p_setter_Callback(hObject, eventdata, handles)
p_slide_Callback(hObject, eventdata, handles)
p_win_Callback(hObject, eventdata, handles)
guidata(hObject, handles);

% --- Executes on button press in p41.
function p41_Callback(hObject, eventdata, handles)
global p;
p = 13;
p_setter_Callback(hObject, eventdata, handles)
p_slide_Callback(hObject, eventdata, handles)
p_win_Callback(hObject, eventdata, handles)
guidata(hObject, handles);

% --- Executes on button press in p42.
function p42_Callback(hObject, eventdata, handles)
global p;
p = 14;
p_setter_Callback(hObject, eventdata, handles)
p_slide_Callback(hObject, eventdata, handles)
p_win_Callback(hObject, eventdata, handles)
guidata(hObject, handles);

% --- Executes on button press in p43.
function p43_Callback(hObject, eventdata, handles)
global p;
p = 15;
p_setter_Callback(hObject, eventdata, handles)
p_slide_Callback(hObject, eventdata, handles)
p_win_Callback(hObject, eventdata, handles)
guidata(hObject, handles);

% --- Executes on button press in p44.
function p44_Callback(hObject, eventdata, handles)
global p;
p = 16;
p_setter_Callback(hObject, eventdata, handles)
p_slide_Callback(hObject, eventdata, handles)
p_win_Callback(hObject, eventdata, handles)
guidata(hObject, handles);

% --- Executes on button press in p_new.
function p_new_Callback(hObject, eventdata, handles)

Fifteen;
x = access();
global bcolor;
bcolor = zeros(4);
p_initial_Callback(hObject, eventdata, handles)
guidata(hObject,handles);

% --- Executes on button press in p_initial.
function p_initial_Callback(hObject, eventdata, handles)

x = access();
% set(handles.p_counter,'String','0');
for m=1:1:4
    for n=1:1:4
        s = ['set(handles.p' int2str(m) int2str(n) ',''BackgroundColor'',''white'')'];
        eval(s);
        if x(m,n) ~= 0
            s = ['set(handles.p' int2str(m) int2str(n) ',''String'',x(' int2str(m) ',' int2str(n) '))'];
            eval(s);
        else
            s = ['set(handles.p' int2str(m) int2str(n) ',''String'',x(' int2str(m) ',' int2str(n) '))'];
            eval(s);
            s = ['set(handles.p' int2str(m) int2str(n) ',''BackgroundColor'',''black'')'];
            eval(s);
        end
    end
end
guidata(hObject,handles);

% --- Executes on button press in p_slide.
function p_slide_Callback(hObject, eventdata, handles)
global bcolor;
global row;
global col;
global row_p;
global col_p;
global counter;
for m=1:1:4
    for n=1:1:4
        s = ['get(handles.p' int2str(m) int2str(n) ',''BackgroundColor'');'];
        z = eval(s);
        bcolor(m,n) = z(1);
        if bcolor(m,n) == 0
            row = m;
            col = n;
        end
    end
end
guidata(hObject,handles);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if limit(row_p,col_p-1) && ~bcolor(row_p,col_p-1)
%     stra = get(handles.p11,'String');
    s1 = ['get(handles.p' int2str(row_p) int2str(col_p) ',''String'');'];
    stra = eval(s1);
%     strb = get(handles.p12,'String');
    s2 = ['get(handles.p' int2str(row_p) int2str(col_p-1) ',''String'');'];
    strb = eval(s2);
    temp = stra;
    stra = strb;
    strb = temp;
%     set(handles.p11,'String',stra);
    s3 = ['set(handles.p' int2str(row_p) int2str(col_p) ',''String'',' stra ');'];
    eval(s3);
%     set(handles.p12,'String',strb);
    s4 = ['set(handles.p' int2str(row_p) int2str(col_p-1) ',''String'',' strb ');'];
    eval(s4);
%     bcolor(1,1) = 0;
%     bcolor(1,2) = 1;
    bcolor(row_p,col_p) = 0;
    bcolor(row_p,col_p-1) = 1;
%    set(handles.p11,'BackgroundColor','black');
    s5 = ['set(handles.p' int2str(row_p) int2str(col_p) ',''BackgroundColor'',''black'');'];
    eval(s5);
%     set(handles.p12,'BackgroundColor','white');
    s6 = ['set(handles.p' int2str(row_p) int2str(col_p-1) ',''BackgroundColor'',''white'')'];
    eval(s6);
    counter = counter + 1;
    set(handles.p_counter,'String',counter);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if limit(row_p,col_p+1) && ~bcolor(row_p,col_p+1)
    s1 = ['get(handles.p' int2str(row_p) int2str(col_p) ',''String'');'];
    stra = eval(s1);
    s2 = ['get(handles.p' int2str(row_p) int2str(col_p+1) ',''String'');'];
    strb = eval(s2);
    temp = stra;
    stra = strb;
    strb = temp;
    s3 = ['set(handles.p' int2str(row_p) int2str(col_p) ',''String'',' stra ');'];
    eval(s3);
    s4 = ['set(handles.p' int2str(row_p) int2str(col_p+1) ',''String'',' strb ');'];
    eval(s4);
    bcolor(row_p,col_p) = 0;
    bcolor(row_p,col_p+1) = 1;
    s5 = ['set(handles.p' int2str(row_p) int2str(col_p) ',''BackgroundColor'',''black'');'];
    eval(s5);
    s6 = ['set(handles.p' int2str(row_p) int2str(col_p+1) ',''BackgroundColor'',''white'')'];
    eval(s6);
    counter = counter + 1;
    set(handles.p_counter,'String',counter);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if limit(row_p-1,col_p) && ~bcolor(row_p-1,col_p)
    s1 = ['get(handles.p' int2str(row_p) int2str(col_p) ',''String'');'];
    stra = eval(s1);
    s2 = ['get(handles.p' int2str(row_p-1) int2str(col_p) ',''String'');'];
    strb = eval(s2);
    temp = stra;
    stra = strb;
    strb = temp;
    s3 = ['set(handles.p' int2str(row_p) int2str(col_p) ',''String'',' stra ');'];
    eval(s3);
    s4 = ['set(handles.p' int2str(row_p-1) int2str(col_p) ',''String'',' strb ');'];
    eval(s4);
    bcolor(row_p,col_p) = 0;
    bcolor(row_p-1,col_p) = 1;
    s5 = ['set(handles.p' int2str(row_p) int2str(col_p) ',''BackgroundColor'',''black'');'];
    eval(s5);
    s6 = ['set(handles.p' int2str(row_p-1) int2str(col_p) ',''BackgroundColor'',''white'')'];
    eval(s6);
    counter = counter + 1;
    set(handles.p_counter,'String',counter);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if limit(row_p+1,col_p) && ~bcolor(row_p+1,col_p)
    s1 = ['get(handles.p' int2str(row_p) int2str(col_p) ',''String'');'];
    stra = eval(s1);
    s2 = ['get(handles.p' int2str(row_p+1) int2str(col_p) ',''String'');'];
    strb = eval(s2);
    temp = stra;
    stra = strb;
    strb = temp;
    s3 = ['set(handles.p' int2str(row_p) int2str(col_p) ',''String'',' stra ');'];
    eval(s3);
    s4 = ['set(handles.p' int2str(row_p+1) int2str(col_p) ',''String'',' strb ');'];
    eval(s4);
    bcolor(row_p,col_p) = 0;
    bcolor(row_p+1,col_p) = 1;
    s5 = ['set(handles.p' int2str(row_p) int2str(col_p) ',''BackgroundColor'',''black'');'];
    eval(s5);
    s6 = ['set(handles.p' int2str(row_p+1) int2str(col_p) ',''BackgroundColor'',''white'')'];
    eval(s6);
    counter = counter + 1;
    set(handles.p_counter,'String',counter);
end
guidata(hObject,handles);

function z = limit(row,col)

z1 = 0;
z2 = 0;
if row>0
    if row<5
        z1 = 1;
    end
end
if col>0
    if col<5
        z2 = 1;
    end
end
z = z1 && z2;

% --- Executes on button press in p_setter.
function p_setter_Callback(hObject, eventdata, handles)
global row_p;
global col_p;
global p;
switch p
    case 1
            row_p = 1;
            col_p = 1;
    case 2
            row_p = 1;
            col_p = 2;
    case 3
            row_p = 1;
            col_p = 3;
    case 4
            row_p = 1;
            col_p = 4;
    case 5
            row_p = 2;
            col_p = 1;
    case 6
            row_p = 2;
            col_p = 2;
    case 7
            row_p = 2;
            col_p = 3;
    case 8
            row_p = 2;
            col_p = 4;
    case 9
            row_p = 3;
            col_p = 1;
    case 10
            row_p = 3;
            col_p = 2;
    case 11
            row_p = 3;
            col_p = 3;
    case 12
            row_p = 3;
            col_p = 4;
    case 13
            row_p = 4;
            col_p = 1;
    case 14
            row_p = 4;
            col_p = 2;
    case 15
            row_p = 4;
            col_p = 3;
    case 16
            row_p = 4;
            col_p = 4;
end
guidata(hObject,handles);

% --- Executes on button press in p_win.
function p_win_Callback(hObject, eventdata, handles)
p = zeros(4);
for m=1:1:4
    for n=1:1:4
        s = ['get(handles.p' int2str(m) int2str(n) ',''String'');'];
        p(m,n) = str2double(eval(s));
    end
end
count = 0;
if p(1,1) == 1
    flag = 1;
    count = count + 1;
else flag = 0;
end

if p(1,2) == 2
    flag = 1;
    count = count + 1;
else flag = 0;
end

if p(1,3) == 3
    flag = 1;
    count = count + 1;
else flag = 0;
end

if p(1,4) == 4
    flag = 1;
    count = count + 1;
else flag = 0;
end

if p(2,1) == 5
    flag = 1;
    count = count + 1;
else flag = 0;
end

if p(2,2) == 6
    flag = 1;
    count = count + 1;
else flag = 0;
end

if p(2,3) == 7
    flag = 1;
    count = count + 1;
else flag = 0;
end

if p(2,4) == 8
    flag = 1;
    count = count + 1;
else flag = 0;
end

if p(3,1) == 9
    flag = 1;
    count = count + 1;
else flag = 0;
end

if p(3,2) == 10
    flag = 1;
    count = count + 1;
else flag = 0;
end

if p(3,3) == 11
    flag = 1;
    count = count + 1;
else flag = 0;
end

if p(3,4) == 12
    flag = 1;
    count = count + 1;
else flag = 0;
end

if p(4,1) == 13
    flag = 1;
    count = count + 1;
else flag = 0;
end

if p(4,2) == 14
    flag = 1;
    count = count + 1;
else flag = 0;
end

if p(4,3) == 15
    flag = 1;
    count = count + 1;
else flag = 0;
end

if p(4,4) == 0
    flag = 1;
    count = count + 1;
else flag = 0;
end

time = int2str(round(toc));
set(handles.p_clock,'String',time);
if flag == 1 && count == 16
    msgbox('Congratulations you have won !!!','Fifteen','help');
end
guidata(hObject,handles);


function mpc = t_case9_opfv2
%T_CASE9_OPFV2   Power flow data for 9 bus, 3 generator case, with OPF data.
%   Please see CASEFORMAT for details on the case file format.

%   MATPOWER

%% MATPOWER Case Format : Version 2
mpc.version = '2';

%%-----  Power Flow Data  -----%%
%% system MVA base
mpc.baseMVA = 100;

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
mpc.bus = [
	1	3	0	0	0	0	1	1	0	345	1	1.1	0.9;
	2	2	0	0	0	0	1	1	0	345	1	1.1	0.9;
	30	2	0	0	0	0	1	1	0	345	1	1.1	0.9;
	4	1	0	0	0	0	1	1	0	345	1	1.1	0.9;
	5	1	90	30	0	0	1	1	0	345	1	1.1	0.9;
	6	1	0	0	0	0	1	1	0	345	1	1.1	0.9;
	7	1	100	35	0	0	1	1	0	345	1	1.1	0.9;
	8	1	0	0	0	0	1	1	0	345	1	1.1	0.9;
	9	1	125	50	0	0	1	1	0	345	1	1.1	0.9;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin	Pc1	Pc2	Qc1min	Qc1max	Qc2min	Qc2max	ramp_agc	ramp_10	ramp_30	ramp_q	apf
mpc.gen = [
	1	0	0	300	-300	1	100	1	250	90	0	0	0	0	0	0	0	0	0	0	0;
	30	85	0	300	-300	1	100	1	270	10	0	200	-30	30	-15	15	0	0	0	0	0;
	2	163	0	300	-300	1	100	1	300	10	0	200	-20	20	-10	10	0	0	0	0	0;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status	angmin	angmax
mpc.branch = [
	1	4	0	0.0576	0	0	250	250	0	0	1	-360	2.48;
	4	5	0.017	0.092	0.158	0	250	250	0	0	1	-360	360;
	5	6	0.039	0.17	0.358	150	150	150	0	0	1	-360	360;
	30	6	0	0.0586	0	0	300	300	0	0	1	-360	360;
	6	7	0.0119	0.1008	0.209	40	150	150	0	0	1	-360	360;
	7	8	0.0085	0.072	0.149	250	250	250	0	0	1	-360	360;
	8	2	0	0.0625	0	250	250	250	0	0	1	-360	360;
	8	9	0.032	0.161	0.306	250	250	250	0	0	1	-360	360;
	9	4	0.01	0.085	0.176	250	250	250	0	0	1	-2	360;
];

%%-----  OPF Data  -----%%
%% generator cost data
%	1	startup	shutdown	n	x1	y1	...	xn	yn
%	2	startup	shutdown	n	c(n-1)	...	c0
mpc.gencost = [
	1	0	0	4	0	0	100	2500	200	5500	250	7250;
	1	0	0	3	0	0	200	3000	300	5000	0	0;
	2	0	0	2	24.035	-403.5	0	0	0	0	0	0;
];


function sVF = rdivide(sVF, sF2)
%
% Syntax
%   sVF = sVF ./ sF
%   sVF = sVF ./ a
%
% Input
%  sVF - @S2VectorFieldTri
%  sF  - @S2Fun
%  a   - double
%
% Output
%  sF - @S2VectorFieldTri
%

if isnumeric(sF2)
  sVF.values = sVF.values ./ sF2;
else
  sVF.values = sVF.values ./ sF2.values;
end

end


function boundary = component_boundary(grid, ndx)

if length(ndx) == 2
  ndx = sub2ind(size(grid), ndx(1), ndx(2));
end

filled = imfill(~grid, ndx, 8);
component = ~(xor(filled,grid));
boundary = iris.terrain_grid.find_boundary(component);

% figure(21)
% subplot(411)
% imshow(grid)
% subplot(412)
% imshow(component)
% subplot(413)
% imshow(fast_boundary);
% subplot(414)
% imshow(boundary);
% assert(all(all(fast_boundary == boundary)));


function [v,n]=cutvar(t,j)
%CUTVAR Variables used for branches in decision tree.
%   V=CUTVAR(T) returns an N-element cell array V of the names of the
%   variables used for branching in each node of the tree T, where N
%   is the number of nodes in the tree.  These variables are sometimes
%   known as cut variables.  For non-branch (leaf) nodes, V contains
%   an empty string.
%
%   V=CUTVAR(T,J) takes an array J of node numbers and returns the cut
%   variables for the specified nodes.
%
%   [V,NUM]=CUTVAR(...) also returns the number NUM of each variable.
%
%   See also CLASSREGTREE, CLASSREGTREE/NUMNODES, CLASSREGTREE/CHILDREN.

%   Copyright 2006-2007 The MathWorks, Inc.
%   $Revision: 1.1.6.2 $  $Date: 2007/02/15 21:48:07 $

if nargin>=2 && ~validatenodes(t,j)
    error('stats:classregtree:parent:InvalidNode',...
          'J must be an array of node numbers or a logical array of the proper size.');
end

% Get variable number, taking care that negative values may appear to
% indicate categorical cuts
if nargin<2
    n = abs(t.var);
else
    n = abs(t.var(j,:));
end

% Get variable name
v = repmat({''},numel(n),1);
mask = n>0;
v(mask) = t.names(n(mask));


function cameras = loadcameradata(dataDir, idx)
%LOADCAMERADATA: Load the dino data
%
%   CAMERAS = LOADCAMERADATA() loads the dinosaur data and returns a
%   structure array containing the camera definitions. Each camera contains
%   the image, internal calibration and external calibration.
%
%   CAMERAS = LOADCAMERADATA(IDX) loads only the specified file indices.
%
%   Example:
%   >> cameras = loadcameradata(1:3);
%   >> showcamera(cameras)
%
%   See also: SHOWCAMERA

%   Copyright 2005-2009 The MathWorks, Inc.
%   $Revision: 1.0 $    $Date: 2006/06/30 00:00:00 $

if nargin<2
    idx = 1:36;
end

cameras = struct( ...
    'Image', {}, ...
    'P', {}, ...
    'K', {}, ...
    'R', {}, ...
    'T', {}, ...
    'Silhouette', {} );

%% First, import the camera Pmatrices
rawP = load( fullfile( dataDir, 'dino_Ps.mat') );

%% Now loop through loading the images
tmwMultiWaitbar('Loading images',0);
for ii=idx(:)'
    % We try both JPG and PPM extensions, trying JPEG first since it is
    % the faster to load
    filename = fullfile( dataDir, sprintf( 'viff.%03d.jpg', ii ) );
    if exist( filename, 'file' )~=2
        % Try PPM
        filename = fullfile( dataDir, sprintf( 'viff.%03d.ppm', ii ) );
        if exist( filename, 'file' )~=2
            % Failed
            error( 'SpaceCarving:ImageNotFound', ...
                'Could not find image %d (''viff.%03d.jpg/.ppm'')', ...
                ii, ii );
        end
    end

    [K,R,t] = spacecarving.decomposeP(rawP.P{ii});
    cameras(ii).rawP = rawP.P{ii};
    cameras(ii).P = rawP.P{ii};
    cameras(ii).K = K/K(3,3);
    cameras(ii).R = R;
    cameras(ii).T = -R'*t;
    cameras(ii).Image = imread( filename );
    cameras(ii).Silhouette = [];
    tmwMultiWaitbar('Loading images',ii/max(idx));
end
tmwMultiWaitbar('Loading images','close');


%SCATTERD Display scatterplot
%
%   H = SCATTERD(A)
%   H = SCATTERD(A,DIM,S,CMAP,FONTSIZE,'label','both','legend','gridded')
%
% INPUT
%   A     Dataset or matrix
%   DIM   Number of dimensions: 1,2 or 3 (optional; default: 2)
%   S     String specifying the colors and markers (optional)
%   CMAP  Matrix with a color map (optional)
%
% OUTPUT
%   H     Vector of handles
%
% DESCRIPTION
% SCATTERD(A) displays a 2D scatterplot of the first two features of the
% dataset A. If the number of dimensions DIM is specified (1..3), it plots
% the first D features in a D-dimensional plot (D<4). If the plot string S
% is provided, e.g. S = 'w+', all points are plotted accordingly. If given,
% different plot strings are used for different classes. See PLOT for the
% specification of plot strings.
%
% If CMAP is specified, the color of the object symbols is determined by
% CMAP indexed by the object labels. A colormap has the size [C x 3], where
% C is the number of classes. The three components of CMAP(I,:) determine
% the red, green and blue components of the color. For instance:
%
%   MAP = HSV; [M,K] = SIZE(A); LABELS = CEIL(64*[1:M]'/M);
%   A = DATASET(A,LABELS); SCATTERD(A,'.','COLORMAP',MAP);
%
% This may be used for tracking ordered objects.
%
% FONTSIZE may be a vector with three elements: fontsize, markersize and
% size of the label font in case of a label plot.
%
% Various other options are:
%   'LABEL'  : plot labels instead of symbols
%   'BOTH'   : plot labels next to each sample
%   'LEGEND' : place a legend in the figure
%   'GRIDDED': make a grid of 2D scatterplots of each pair of features
%
% All the parameters, except for the dataset A can be specified in any
% order or can be left out.
%
% Classifiers can be plot in the scatterplot by PLOTC.
% Note that PLOTC does not work for 1D and 3D scatterplots.
%
% EXAMPLES
% See PREX_CONFMAT, PREX_DENSITY, PREX_PLOTC, PREX_MCPLOT.
%
% SEE ALSO
% DATASETS, COLORMAP, PLOT, PLOTC

% Copyright: D. de Ridder, R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Sciences, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

% $Id: scatterd.m,v 1.5 2009/01/04 21:43:17 duin Exp $

% REVISIONS
% DR1 - Dick, 05-10-2004
% Added plotting of unlabeled data as 'k.'.

function handle = scatterd(a,p1,p2,p3,p4,p5,p6,p7,p8)

	prtrace(mfilename);

	% Defaults
	d = min(size(a,2),2);		% Dimensionality of plot
	s = []; 					% Plot symbol(s)
	map = []; 					% Color map
	plotlab = 0;				% Put text labels instead of or next to samples
	plotsym = 1;				% Plot symbols for samples
	plotlegend = 0; 		    % Plot legend
	gridded = 0;				% Make a gridded plot
	gridrun = 0;				% Inner loop in a gridded plot?
	font_size = [];
	mark_size = [];
	lab_size  = [];
	hold_axis = ishold; % A flag to check if 'hold on' is set for the current axis

	a = prdataset(a); 		% Allow for a non-dataset data

	if (nargin < 9), par{8} = []; else, par{8} = p8; end
	if (nargin < 8), par{7} = []; else, par{7} = p7; end
	if (nargin < 7), par{6} = []; else, par{6} = p6; end
	if (nargin < 6), par{5} = []; else, par{5} = p5; end
	if (nargin < 5), par{4} = []; else, par{4} = p4; end
	if (nargin < 4), par{3} = []; else, par{3} = p3; end
	if (nargin < 3), par{2} = []; else, par{2} = p2; end
	if (nargin < 2), par{1} = []; else, par{1} = p1; end

	% Set up default values.
	for i = 1:5
		if (~isempty(par{i}))
			if (length(par{i}) == 1) & par{i} < 5 & (~ischar(par{i})) 	% Dimensionality
				d   = par{i}; par{i} = 2; 		% Necessary for gridded: D needs to be 2.
			elseif ((size(par{i},1) > 1) & (size(par{i},2)==3) & (~ischar(par{i}))) 	% Color map
				map = par{i};
			elseif (strcmp(par{i},'label'))
				plotlab = 1; plotsym = 0;
			elseif (strcmp(par{i},'both'))
				plotlab = 1; plotsym = 1;
			elseif (strcmp(par{i},'legend'))
				plotlegend = 1;
			elseif (strcmp(par{i},'gridded'))
				gridded = 1;
				par{i} = 'gridrun'; 					% Necessary for gridded: otherwise an infinite recursion :)
			elseif (strcmp(par{i},'gridrun'))
				gridrun = 1;
			elseif ~isstr(par{i}) & length(par{i}) <= 3 & par{i}(1) >= 5
				font_size = par{i}(1);
				if length(par{i}) >= 2
					mark_size = par{i}(2);
				end
				if length(par{i}) == 3
					lab_size = par{i}(3);
				end
			else
				s	= par{i};
			end
		end
	end

	if (gridrun)
		if isempty(font_size), font_size = 10; end
		if isempty(mark_size), mark_size =  5; end
		if isempty(lab_size),   lab_size =  8; end
	elseif (~hold_axis)
		%clf;
		cla;
		if isempty(font_size), font_size = 16; end
		if isempty(mark_size), mark_size =  7; end
		if isempty(lab_size),   lab_size = 14; end
	else
		if isempty(font_size), font_size = get(gca,'fontsize'); end
		if isempty(mark_size), mark_size = font_size/2; end
		if isempty(lab_size),   lab_size = font_size-2; end
	end

	feats = getfeatlab(a,'string');

	if isempty(feats)
		for i=1:size(a,2)
			feats = strvcat(feats,sprintf('Feature %d',i));
		end
	else
		if size(feats,2) == 1
			feats = [repmat('Feature ',size(feats,1),1) feats];
		end
	end

	if (gridded)
		clf;
		gs = size(a,2);
		for i = 1:gs
			for j = 1:gs
				subplot(gs,gs,(i-1)*gs+j);
				gridrun = 1;
				h = feval(mfilename,a(:,[j i]),par{1},par{2},par{3},par{4},par{5},par{6},par{7});
				gridrun = 0;
				if (i~=gs), xlabel(''); end
				if (j~=1),  ylabel(''); end
			end
			subplot(111); 		% Takes care of clf for the next scatterplot.
		end
		return;
	end

	if (isa(a,'prdataset')) & (~isempty(getlablist(a)))
        [m,k,c] = getsize(a);
        cs_a = classsizes(a);
        classes_present = cs_a > 0;
		lab = getnlab(a);
		lablist = getlablist(a,'string');
		ident = getident(a); %DXD: needed for prcursor
		dataset_name = getname(a);
		a = double(a);
	else
		[m,k] = size(a);
		lab = ones(m,1);
		ident = (1:m)'; %DXD: needed for prcursor
		dataset_name = [];
		c = 1;
		a = double(a);
	end

	% Character string defining the plotting setup in terms of symbols and colors.
	if (isempty(s))
		vers = version;
		if (str2num(vers(1)) < 5)
			col = 'brmw';
			sym = ['+*xo.']';
			i   = [1:20];
			ss  = [col(i-floor((i-1)/4)*4)' sym(i-floor((i-1)/5)*5)];
		else
			col = 'brmk';
			sym = ['+*oxsdv^<>p']';
			i   = [1:44];
			ss  = [col(i-floor((i-1)/4)*4)' sym(i-floor((i-1)/11)*11)];
		end
    ss  = ['k.'; ss];  								% DR1 - Add symbol for "unlabeled" data.
		[ms,ns] = size(ss);
		if ms == 1, ss = setstr(ones(m,1)*ss); end
	else
		if size(s,1) == 1
			ss = repmat(s,c,1); s = [];
		else
			ss = s; s = [];
		end
    ss  = char('k.', ss);  								% DR1 - Add symbol for "unlabeled" data.
	                                                %DXD - changed [.] into char(.)
	end

	% Define some 'space' OY to be added around the data plotted in symbols.
	oy = zeros(1,3);
	if (plotsym)
		oy = 0.02*(max(a)-min(a));
	else
		s = 'w.'; 				% Plot white spot instead of symbols.
	end
	oy(2) = 0;

	% Make a plot.
	lhandle = []; thandle = [];

    classes_present_idx = find(classes_present);

    [dummy cs_a_idx] = sort(cs_a(classes_present_idx),'descend');

  % Also plot label "0" (unlabeled).
	for i = [0 classes_present_idx(cs_a_idx) ]
		J = find(lab==i);
		if (isempty(s)), symbol = ss(i+1,:); else, symbol = s; end
		if ((d == 1) & ~isempty(J))
			h = plot(a(J,1),zeros(length(J),1),symbol);
			hold on;
			set(h,'markersize',mark_size);
			lhandle = [lhandle h];
			if (plotlab)
				for j = 1:length(J)
					h = text(a(J(j),1)+oy(1),oy(2),lablist(lab(J(j)),:));
					set(h,'fontsize',lab_size);
					thandle = [thandle h]; if (~isempty(map)), set (h, 'color', map(i+1,:)); end
				end
			end
		elseif ((d == 2) & ~isempty(J))
			h = plot(a(J,1),a(J,2),symbol);
			hold on;
			set(h,'markersize',mark_size);
			lhandle = [lhandle h];
			if (plotlab)
				for j = 1:length(J)
					h = text(a(J(j),1)+oy(1),a(J(j),2)+oy(2),lablist(lab(J(j)),:));
					set(h,'fontsize',lab_size);
					thandle = [thandle h]; if (~isempty(map)), set (h, 'color', map(i+1,:)); end
				end
			end
		elseif (~isempty(J))
			h = plot3(a(J,1),a(J,2),a(J,3),symbol);
			hold on;
			set(h,'markersize',mark_size);
			lhandle = [lhandle h];
			if (plotlab)
				for j = 1:length(J)
					h = text(a(J(j),1)+oy(1),a(J(j),2)+oy(2),a(J(j),3)+oy(3),lablist(lab(J(j)),:));
					set(h,'fontsize',lab_size);
					thandle = [thandle h]; if (~isempty(map)), set (h, 'color', map(i+1,:)); end
				end
			end
		end
		%DXD: store the object identifiers in the userdata such that you
		%can retrieve them by prcursor:
		if ~isempty(J)
			ud = get(h,'UserData');
			ud.ident = ident(J);
			set(h,'UserData',ud);
		end
    end

	if (plotsym)
		if (~isempty(map))
			for i = 0:c, set (lhandle(i+1), 'color', map(i+1,:)); end
		end
		if (plotlegend),
			[ht, hl] = legend(lhandle(:), [ num2str(cs_a(classes_present_idx(cs_a_idx))') repmat(' ', sum(classes_present),1) lablist(classes_present_idx(cs_a_idx),:) ]);
			hl = hl(:)';
			%set(hl(1:2*c),'markersize',mark_size);
			lhandle = [lhandle hl];
			thandle = [thandle ht(:)'];
		end
	end

	% !%_%*!_% Matlab

	set(gca,'fontsize',font_size);

	if (~hold_axis)
		dd = (max(a) - min(a))*0.05;		% offset, avoiding points on plot box.
		J = find(dd==0);
		dd(J) = dd(J)+1;

%feats
		if (d == 1),
			axis ([min(a(:,1))-dd(1) max(a(:,1))+dd(1) -0.5 0.5]);
			hx = xlabel(feats(1,:));
			thandle = [thandle hx];
		elseif (d == 2),
			axis ([min(a(:,1))-dd(1) max(a(:,1))+dd(1) min(a(:,2))-dd(2) max(a(:,2))+dd(2)]);
			hx = xlabel(feats(1,:)); hy = ylabel(feats(2,:));
			thandle = [thandle(:)' hx hy];
			view(2);
		else		% D = 3
			axis ([min(a(:,1))-dd(1) max(a(:,1))+dd(1) min(a(:,2))-dd(2) max(a(:,2))+dd(2) min(a(:,3))-dd(3) max(a(:,3))+dd(3)]);
			hx = xlabel(feats(1,:)); hy = ylabel(feats(2,:)); hz = zlabel(feats(3,:));
			thandle = [thandle hx hy hz];
			view(3);
		end
	end

	if (~gridrun) & (length(get(gcf,'children')) < 3 & any(get(gca,'position')>0.80))
		set(gca,'position',[0.13 0.13 0.79 0.78]); % make axis labels visible
	end

	if (~gridrun) & (~isempty(dataset_name))
		title(dataset_name);
	end

	hold off;

	if (nargout > 0)
		handle = [lhandle thandle];
	end

return;


function scimatsk = scimat_skeleton_prune(scimatsk, scimat, minlen, lratio, alphamax, p)
% SCIMAT_SKELETON_PRUNE  Prune branches in a segmentation's skeletonization
%
% This function prunes the leaves of a segmentation skeleton in three
% steps:
%
%   1. Erode clumps of voxels to look like branches
%
%   2. Prune very short leaves (iteratively until no more leaves can be
%      pruned)
%
%   3. Prune leaves created by artifacts in the segmentation. Spurious
%      leaves are those roughly as long as the local radius of the main
%      branch they are attached to (iteratively until no more leaves can
%      be pruned)
%
%
% SCIMATPR = scimat_skeleton_prune(SCIMATSK)
% SCIMATPR = scimat_skeleton_prune(SCIMATSK, [], MINLEN)
%
%   This syntax runs steps 1 and 2 only.
%
%   SCIMATSK is an SCIMAT struct. SCIMATSK.data contains the result of
%   running a skeletonization algorithm on a binary segmentation SCIMAT,
%   e.g.
%
%     >> scimatsk = scimat;
%     >> scimatsk.data = itk_imfilter('skel', scimat);
%
%   MINLEN is a scalar with the minimum length in voxels for a leaf. Any
%   leaf shorter than MINLEN will be pruned. By default, MINLEN = 5 voxel.
%
%
% SCIMATPR = scimat_skeleton_prune(SCIMATSK, SCIMAT, MAXCLUMP, MINLEN, LRATIO, ALPHAMAX, P)
%
%   This syntax runs steps 1, 2 and 3.
%
%   SCIMAT is the binary segmentation mentioned above.
%
%   LRATIO is a scalar. Leaves with L/R < LRATIO will be pruned, where
%   L is the length from the bifurcation to the tip of the leaf. By
%   default, LRATIO=1.2.
%
%   ALPHAMAX and P are merging parameters. See the help of function
%   skeleton_label for details. If merging is not enabled, then no leaves
%   will be pruned in step 3.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2011, 2014 University of Oxford
% Version: 0.5.1
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
narginchk(1, 6);
nargoutchk(0, 1);

if ~isstruct(scimatsk)
    error('SCIMATSK must be an SCIMAT struct')
end

% defaults
if (nargin < 2)
    scimat = [];
end
if (nargin < 3 || isempty(minlen))
    minlen = 5;
end
if (nargin < 4 || isempty(lratio))
    lratio = 1.2;
end
if (nargin < 5 || isempty(alphamax))
    alphamax = -Inf; % don't merge by default
end
if (nargin < 6 || isempty(p))
    p = 1.0; % don't smooth by default
end

%% Step 1: removal of big clumps of voxels

% if we have no voxels to prune, don't waste time processing
if (nnz(scimatsk.data) == 0)
    return
end

% get sparse matrix of distances between voxels. To label the skeleton we
% don't care about the actual distances, just need to know which voxels are
% connected to others. Actual distances are needed to parameterize the
% branches, though
[dsk, dictsk, idictsk] = seg2dmat(scimatsk.data, 'seg', ...
    [scimatsk.axis.spacing]);

% find bifurcation voxels

% compute degree of each skeleton voxel
deg = sum(dsk > 0, 2);

% get distance matrix index of the bifurcation voxels
bifidx = deg >= 3;

% matrix index => image index
bifidx = idictsk(bifidx);

% label connected components of skeleton branches

% remove bifurcation voxels from original skeleton
scimatsk.data(bifidx) = 0;

% get connected components in the image
cc = bwconncomp(scimatsk.data);

% make size vector always have size(3)
if length(cc.ImageSize) == 2
    cc.ImageSize = [cc.ImageSize 1];
end

% connectivity between branches and bifurcation clumps,
% and find which branches should be merged together

% tag each branch with its label
scimatsk.data = labelmatrix(cc);

% create empty image volume and add only bifurcation voxels
sk2 = zeros(cc.ImageSize, 'uint8');
sk2(bifidx) = 1;

% compute connected components to obtain clumps of bifurcation voxels
bifcc = bwconncomp(sk2);

% loop all the bifurcation clumps, to find which branches are neighbours of
% each other
for I = 1:bifcc.NumObjects

    %% find which branches are connected to which bifurcation clumps

    % get voxels in the bifurcation clump
    % linear index -> row, col, slice
    [r, c, s] = ind2sub(cc.ImageSize, bifcc.PixelIdxList{I});

    % get a box 1 voxel bigger than the clump
    r0 = max(1, min(r) - 1);
    c0 = max(1, min(c) - 1);
    s0 = max(1, min(s) - 1);
    rend = min(cc.ImageSize(1), max(r) + 1);
    cend = min(cc.ImageSize(2), max(c) + 1);
    send = min(cc.ImageSize(3), max(s) + 1);

    % extract that box from the volume with the branches
    boxbr = scimatsk.data(r0:rend, c0:cend, s0:send);

    % create a box for the bifurcation clump
    boxbif = zeros(size(boxbr), class(sk2));
    r = r - r0 + 1;
    c = c - c0 + 1;
    s = s - s0 + 1;
    boxbif(sub2ind(size(boxbif), r, c, s)) = 1;

    % create a box with the same size
    box  = zeros(size(boxbif));

    % tag as TODO=2 branch voxels
    box(boxbr ~= 0) = 2;

    % add to the vox the bifurcation clump voxels
    box(boxbif == 1) = 1;

    % dilate the clump 1 voxel
    box = bwregiongrow(box, 2, [], 1);

    % get the branches that the dilated bifurcation clump overlaps
    idx = double(boxbr(box == 1));

    % because in sk bifurcation voxels were removed, we can have "0" values
    % in idx. Remove them
    idx = idx(idx ~= 0);

    % number of branches
    N = length(idx);

    % in order to merge, the bifurcation needs to have at least 2
    % branches
    if (N < 2)
        continue
    end

    % get all combinations of pairs of branches that share this clump
    idx = nchoosek(idx, 2);

    % get voxel indices for bifurcation clump
    bif = bifcc.PixelIdxList{I};

    % duplicate the list. bif0 is going to be the list of superfluous
    % bifurcation voxels
    bif0 = bif;

    % loop each pair of branches combination
    for J = 1:size(idx, 1)

        % get voxel indices for each branch
        br0 = cc.PixelIdxList{idx(J, 1)};
        br1 = cc.PixelIdxList{idx(J, 2)};

        % merge and sort both branches and the bifurcation clump
        br = sort_branch([br0(:); bif(:); br1(:)], ...
            dsk, dictsk, idictsk);

        % remove from the list of superfluous bifurcation voxels those
        % that are needed to connect the two branches
        bif0 = setdiff(bif0, br);

    end

    % remove from the skeleton the list of superfluous bifurcation
    % voxels
    bifcc.PixelIdxList{I} = setdiff(bif, bif0);

end

% convert labelled skeleton to binary mask
scimatsk.data = uint8(scimatsk.data ~= 0);

% add bifurcation voxels to skeleton binary mask
scimatsk.data(cat(1, bifcc.PixelIdxList{:})) = 1;

%% Step 2: pruning of very short leaf branches
while (1)

    % compute skeleton labelling
    [~, cc] = skeleton_label(scimatsk.data, [], [scimatsk.axis.spacing]);

    % get number of voxels in each branch
    n = cellfun(@(x) length(x), cc.PixelIdxList);

    % find leaf-branches that are shorter than the minimum length
    idx1 = find(n < minlen & cc.IsLeaf);

    % remove short branches from the segmentation
    scimatsk.data(cat(1, cc.PixelIdxList{idx1})) = 0;

    % recompute the skeleton labelling
    [~, ~, bifcc, mcon] = skeleton_label(scimatsk.data, [], [scimatsk.axis.spacing]);

    % find bifurcation clusters that are connected to 0 or 1 branches
    idx2 = find(sum(mcon, 1) < 2);

    % remove those bifurcation clumps, because they are not connecting
    % branches, they are either floating alone in space, or terminating a
    % branch
    scimatsk.data(cat(1, bifcc.PixelIdxList{idx2})) = 0;

    % if no bifurcation clumps were found to be removed, stop the
    % algorithm, because that means that no new short leaf-braches can be
    % found either
    if (isempty(idx2))
        break;
    end

end

%% Step 3: pruning of leaf branches produced by segmentation artifacts

% skip if no full segmentation is provided
if isempty(scimat)
    return
end

% repeat the process until no branches are removed
atleastonepruning = true;
while (atleastonepruning)

    % if there are no prunings in this iterations, we stop
    atleastonepruning = false;

    % label the segmentation using multiple merging at every bifurcation
    % clump
    [scimat.data, cc, ~, mcon, ~, cc2] = ...
        skeleton_label(scimatsk.data, scimat.data, [scimat.axis.spacing], ...
        alphamax, p, false);

    % measure the stats of every merged branch
    stats2 = scimat_seg2label_stats(scimat, cc2, p);

    % loop each branch in the list of merged branches
    for I = 1:cc2.NumObjects

        % we consider the current branch a main branch, and any branch
        % coming out of it, a secondary branch

        % compute the major radius of the main branch
        r = sqrt(4 * stats2.Var(2, I));

        % loop consecutive pairs of segments in the main branch
        for J = 1:length(cc2.MergedBranches{I})-1

            % get the bifurcation clump between both segments
            bif = cc2.MergedBifClumps{I}(J);

            % get secondary branches attached to the bifurcation clump
            % (usually, there's only 1 secondary branch, but there can be more)
            bn = setdiff(find(mcon(:, bif)), cc2.MergedBranches{I}(J:J+1));

            % keep only secondary branches that are leaf-branches
            bn = bn(cc.IsLeaf(bn));

            % skip to next segment if there are no valid leaf secondary
            % branches
            if isempty(bn)
                continue
            end

            % compute distance between the first and last voxels in the
            % secondary branch, which gives a better estimate of whether the
            % branch protudes from the main vessel or not, than the branches
            % length (because the branch can be bent)
            len = zeros(size(bn));
            for K = 1:length(len)
                idx = cc.PixelIdxList{bn(K)}([1 end]);
                [r1, c1, s1] = ind2sub(size(scimatsk.data), idx);
                xyz = scimat_index2world([r1(:), c1(:), s1(:)], scimatsk);
                len(K) = sqrt(sum(diff(xyz).^2));
            end

            % get branches that have to be pruned
            toremove = (len / r) <= lratio;
            atleastonepruning = atleastonepruning || any(toremove);

            % if length of the secondary branch is not much larger than the
            % radius of the main branch, we assume that the secondary branch is
            % a segmentation artifact, and remove it
            scimatsk.data(cat(1, ...
                cc.PixelIdxList{bn(toremove)})) = 0;

        end

    end

end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% auxiliary functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% sort a set of voxels so that they form a branch
% idx: voxel indices
% d: distance matrix
function [idx, param] = sort_branch(idx, d, dict, idict)

% number of voxels in the current branch
N = length(idx);

% degenerate case in which the branch has only one voxel
if (N == 1)
    param = 0;
    return
end

% create a "branch distance matrix" for only the voxels in the branch
idx2 = full(dict(idx));
dbr = d(idx2, idx2);

% compute Dijkstra's shortest path from an arbitrary voxel in the
% branch to every other voxel
[dp, ~] = dijkstra(dbr, 1);

% the furthest voxel should be one of the ends of the branch
[~, v0] = max(dp);

% compute shortest path to all other voxels in the branch
%
% note that even for apparently "wire" branches, sometimes we get small
% cycles of 1 voxel, and instead of a "linear" shortest-path, we have a
% tree, so in the next step, some of the voxels are going to be thrown
% away
[dp, parents] = dijkstra(dbr, v0);
[~, v1] = max(dp);

% backtrack the whole branch in order from the furthest point to
% the original extreme point
idx = [];
param = [];
J = 1;
while (v1 ~= 0)
    idx(J) = idict(idx2(v1));
    param(J) = dp(v1);
    v1 = parents(v1);
    J = J + 1;
end

% make sure that cc.PixelIdxList{I} is a column vector
idx = idx(:);

% reorder voxels so that the parameterization increases
% monotonically
idx = idx(end:-1:1);
param = param(end:-1:1);

end


function varargout = plot_directed_edges(V,E,varargin)
  % PLOT_DIRECTED_EDGES Plot direct edges using quiver
  %
  % p = plot_directed_edges(V,E)
  % p = plot_directed_edges(V,E,LINESPEC,...)
  %
  % Inputs:
  %   V  #V by dim list of edge vertiecs
  %   E  #E by 2 list of edges
  % Outputs:
  %   p  plot handle
  %
  % See also: plot_edges
  %
  if isempty(E)
    return;
  end

  switch size(V,2)
  case 2
    q = quiver( ...
      V(E(:,1),1), ...
      V(E(:,1),2), ...
      V(E(:,2),1)-V(E(:,1),1), ...
      V(E(:,2),2)-V(E(:,1),2), ...
      0, ...
      varargin{:});
   otherwise
    error('Unsupported dimension: %d',size(V,2));
   end

  if nargout >= 1
    varargout{1} = p;
  end

end


function C = cell2ColorOrder(colors);
%
% C = cell2ColorOrder(colors);
%
% Convert a cell array containing color signifiers
% into an N x 3 matrix, useable for color orders for
% axes. (e.g., in setLineColors or mybar).
%
% color signifiers can be 1x3 matrices of [R G B]
% values (from 0-1 for each channel), or else one
% of the following characters:
%
%   'r': red [1 0 0]
%   'g': green [0 1 0]
%   'b': blue [0 0 1]
%   'k': black [0 0 0]
%   'y': yellow [1 1 0]
%   'm': magenta [1 0 1]
%   'c': cyan [0 1 1]
%   'w': white [1 1 1]
%   'e' or 'a': gray [.6 .6 .6]
%
%
% ras, 06/05.
if ~iscell(colors), error('Requires a cell input'); end

colors = unNestCell(colors);
C = zeros(length(colors),3);

for i = 1:length(colors)
    if isnumeric(colors{i}) & length(colors{i})==3
        C(i,:) = colors{i};
    elseif ischar(colors{i})
        ch = colors{i}(1);
        switch ch
            case 'r', C(i,:) = [1 0 0];
            case 'g', C(i,:) = [0 1 0];
            case 'b', C(i,:) = [0 0 1];
            case 'k', C(i,:) = [0 0 0];
            case 'y', C(i,:) = [1 1 0];
            case 'm', C(i,:) = [1 0 1];
            case 'c', C(i,:) = [0 1 1];
            case 'w', C(i,:) = [1 1 1];
            case {'e','a'}, C(i,:) = [.6 .6 .6];
            otherwise,
                warning(sprintf('Unrecognized character %s',ch));
        end
    else
        error(sprintf('Entry %i is not a color signifier.',i));
    end
end

return


function [bnet, names] = mk_bat_dbn()
% MK_BAT_DBN Make the BAT DBN
% [bnet, names] = mk_bat_dbn()
% See
% - Forbes, Huang, Kanazawa and Russell, "The BATmobile: Towards a Bayesian Automated Taxi", IJCAI 95
% - Boyen and Koller, "Tractable Inference for Complex Stochastic Processes", UAI98.

names = {'LeftClr', 'RightClr', 'LatAct', 'Xdot', 'InLane', 'FwdAct', ...
      'Ydot', 'Stopped', 'EngStatus', 'FBStatus', ...
      'LeftClrSens', 'RightClrSens', 'TurnSignalSens', 'XdotSens', 'YdotSens', ...
      'FYdotDiffSens', 'FclrSens', 'BXdotSens', 'BclrSens', 'BYdotDiffSens', ...
      'SensorValid', 'FYdotDiff', 'FcloseSlow', 'Fclr', 'BXdot', 'BcloseFast', 'Bclr', 'BYdotDiff'};
ss = length(names);

intrac = {...
      'LeftClr', 'LeftClrSens';
  'RightClr', 'RightClrSens';
  'LatAct', 'TurnSignalSens'; 'LatAct', 'Xdot';
  'Xdot', 'XdotSens';
  'FwdAct', 'Ydot';
  'Ydot', 'YdotSens'; 'Ydot', 'Stopped';
  'EngStatus', 'Ydot'; 'EngStatus', 'FYdotDiff'; 'EngStatus', 'Fclr'; 'EngStatus', 'BXdot';
  'SensorValid', 'XdotSens';   'SensorValid', 'YdotSens';
  'FYdotDiff', 'FYdotDiffSens'; 'FYdotDiff', 'FcloseSlow';
  'FcloseSlow', 'FBStatus';
  'Fclr', 'FclrSens'; 'Fclr', 'FcloseSlow';
  'BXdot', 'BXdotSens';
  'Bclr', 'BclrSens'; 'Bclr', 'BXdot'; 'Bclr', 'BcloseFast';
  'BcloseFast', 'FBStatus';
  'BYdotDiff', 'BYdotDiffSens'; 'BYdotDiff', 'BcloseFast'};
[intra, names] = mk_adj_mat(intrac, names, 1);

interc = {...
      'LeftClr', 'LeftClr'; 'LeftClr', 'LatAct';
  'RightClr', 'RightClr'; 'RightClr', 'LatAct';
  'LatAct', 'LatAct'; 'LatAct', 'FwdAct';
  'Xdot', 'Xdot'; 'Xdot', 'InLane';
  'InLane', 'InLane'; 'InLane', 'LatAct';
  'FwdAct', 'FwdAct';
  'Ydot', 'Ydot';
  'Stopped', 'Stopped';
  'EngStatus', 'EngStatus';
  'FBStatus', 'FwdAct'; 'FBStatus', 'LatAct'};
inter = mk_adj_mat(interc, names, 0);

obs = {'LeftClrSens', 'RightClrSens', 'TurnSignalSens', 'XdotSens', 'YdotSens', 'FYdotDiffSens', ...
      'FclrSens', 'BXdotSens', 'BclrSens', 'BYdotDiffSens'};

for i=1:length(obs)
  onodes(i) = strmatch(obs{i}, names); %stringmatch(obs{i}, names);
end
onodes = sort(onodes);

dnodes = 1:ss;
ns = 2*ones(1,ss); % binary nodes
bnet = mk_dbn(intra, inter, ns, 'discrete', dnodes, 'observed', onodes, 'eclass2', (1:ss)+ss);

% make rnd params
for i=1:2*ss
  bnet.CPD{i} = tabular_CPD(bnet, i);
end


%kvhmed 'Median Filter the Image Using Histogram to Find Median (K1)'
% This MatLab function was automatically generated by a converter (KhorosToMatLab) from the Khoros vhmed.pane file
%
% Parameters:
% InputFile: i 'Input Image ', required: 'input image filename'
% Integer: w 'Window Width ', default: 3: 'window width argument'
% Integer: h 'Window Height', default: 3: 'window height argument'
% OutputFile: o 'Output Image', required: 'output image filename'
%
% Example: o = kvhmed(i, {'i','';'w',3;'h',3;'o',''})
%
% Khoros helpfile follows below:
%
%  PROGRAM
% vhmed - Median Filter the Image Using Histogram to Find Median  (K1)
%
%  DESCRIPTION
% .I vhmed
% computes a two dimensional median filter of width w and height h over
% the given image.  The median is computed by moving a w by h
% pixel mask over the image and updating the histogram of the mask.
%
% Images must be of data storage type byte.
%
% In rectangular odd windows (masks) the median value is replaced in
% the center pixel location of the window. The default window
% size is 3x3.
%
% The width or height of a window cannot be an even number.
%
%
%
%  EXAMPLES
% vhmed -i input.image -o output.image -w 3 -h 3
%
% This command performs median filtering of width 3 and height 3 on
% input.image and stores the result in output.image.
%
%  "SEE ALSO"
% vqmed(1)
%
%  RESTRICTIONS
% .I vhmed
% does not support even window dimensions and only works on
% data storage type byte.
%
%  REFERENCES
%
%  COPYRIGHT
% Copyright (C) 1993 - 1997, Khoral Research, Inc. ("KRI")  All rights reserved.
%

function varargout = kvhmed(varargin)
if nargin ==0
  Inputs={};arglist={'',''};
elseif nargin ==1
  Inputs=varargin{1};arglist={'',''};
elseif nargin ==2
  Inputs=varargin{1}; arglist=varargin{2};
else error('Usage: [out1,..] = kvhmed(Inputs,arglist).');
end
if size(arglist,2)~=2
  error('arglist must be of form {''ParameterTag1'',value1;''ParameterTag2'',value2}')
 end
narglist={'i', '__input';'w', 3;'h', 3;'o', '__output'};
maxval={0,32,32,0};
minval={0,1,1,0};
istoggle=[0,1,1,0];
was_set=istoggle * 0;
paramtype={'InputFile','Integer','Integer','OutputFile'};
% identify the input arrays and assign them to the arguments as stated by the user
if ~iscell(Inputs)
Inputs = {Inputs};
end
NumReqOutputs=1; nextinput=1; nextoutput=1;
  for ii=1:size(arglist,1)
  wasmatched=0;
  for jj=1:size(narglist,1)
   if strcmp(arglist{ii,1},narglist{jj,1})  % a given argument was matched to the possible arguments
     wasmatched = 1;
     was_set(jj) = 1;
     if strcmp(narglist{jj,2}, '__input')
      if (nextinput > length(Inputs))
        error(['Input ' narglist{jj,1} ' has no corresponding input!']);
      end
      narglist{jj,2} = 'OK_in';
      nextinput = nextinput + 1;
     elseif strcmp(narglist{jj,2}, '__output')
      if (nextoutput > nargout)
        error(['Output nr. ' narglist{jj,1} ' is not present in the assignment list of outputs !']);
      end
      if (isempty(arglist{ii,2}))
        narglist{jj,2} = 'OK_out';
      else
        narglist{jj,2} = arglist{ii,2};
      end

      nextoutput = nextoutput + 1;
      if (minval{jj} == 0)
         NumReqOutputs = NumReqOutputs - 1;
      end
     elseif isstr(arglist{ii,2})
      narglist{jj,2} = arglist{ii,2};
     else
        if strcmp(paramtype{jj}, 'Integer') & (round(arglist{ii,2}) ~= arglist{ii,2})
            error(['Argument ' arglist{ii,1} ' is of integer type but non-integer number ' arglist{ii,2} ' was supplied']);
        end
        if (minval{jj} ~= 0 | maxval{jj} ~= 0)
          if (minval{jj} == 1 & maxval{jj} == 1 & arglist{ii,2} < 0)
            error(['Argument ' arglist{ii,1} ' must be bigger or equal to zero!']);
          elseif (minval{jj} == -1 & maxval{jj} == -1 & arglist{ii,2} > 0)
            error(['Argument ' arglist{ii,1} ' must be smaller or equal to zero!']);
          elseif (minval{jj} == 2 & maxval{jj} == 2 & arglist{ii,2} <= 0)
            error(['Argument ' arglist{ii,1} ' must be bigger than zero!']);
          elseif (minval{jj} == -2 & maxval{jj} == -2 & arglist{ii,2} >= 0)
            error(['Argument ' arglist{ii,1} ' must be smaller than zero!']);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} < minval{jj})
            error(['Argument ' arglist{ii,1} ' must be bigger than ' num2str(minval{jj})]);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} > maxval{jj})
            error(['Argument ' arglist{ii,1} ' must be smaller than ' num2str(maxval{jj})]);
          end
        end
     end
     if ~strcmp(narglist{jj,2},'OK_out') &  ~strcmp(narglist{jj,2},'OK_in')
       narglist{jj,2} = arglist{ii,2};
     end
   end
   end
   if (wasmatched == 0 & ~strcmp(arglist{ii,1},''))
        error(['Argument ' arglist{ii,1} ' is not a valid argument for this function']);
   end
end
% match the remaining inputs/outputs to the unused arguments and test for missing required inputs
 for jj=1:size(narglist,1)
     if  strcmp(paramtype{jj}, 'Toggle')
        if (narglist{jj,2} ==0)
          narglist{jj,1} = '';
        end;
        narglist{jj,2} = '';
     end;
     if  ~strcmp(narglist{jj,2},'__input') && ~strcmp(narglist{jj,2},'__output') && istoggle(jj) && ~ was_set(jj)
          narglist{jj,1} = '';
          narglist{jj,2} = '';
     end;
     if strcmp(narglist{jj,2}, '__input')
      if (minval{jj} == 0)  % meaning this input is required
        if (nextinput > size(Inputs))
           error(['Required input ' narglist{jj,1} ' has no corresponding input in the list!']);
        else
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        end
      else  % this is an optional input
        if (nextinput <= length(Inputs))
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end;
     else
     if strcmp(narglist{jj,2}, '__output')
      if (minval{jj} == 0) % this is a required output
        if (nextoutput > nargout & nargout > 1)
           error(['Required output ' narglist{jj,1} ' is not stated in the assignment list!']);
        else
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
          NumReqOutputs = NumReqOutputs-1;
        end
      else % this is an optional output
        if (nargout - nextoutput >= NumReqOutputs)
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end
     end
  end
end
if nargout
   varargout = cell(1,nargout);
else
  varargout = cell(1,1);
end
global KhorosRoot
if exist('KhorosRoot') && ~isempty(KhorosRoot)
w=['"' KhorosRoot];
else
if ispc
  w='"C:\Program Files\dip\khorosBin\';
else
[s,w] = system('which cantata');
w=['"' w(1:end-8)];
end
end
[varargout{:}]=callKhoros([w 'vhmed"  '],Inputs,narglist);


function pf = loadPoleFigure_scintag(fname,varargin)
% import data fom scintag ascii file
%
% Syntax
%   pf = loadPoleFigure_scintag(fname)
%
% Input
%  fname    - filename
%
% Output
%  pf - vector of @PoleFigure
%
% See also
% ImportPoleFigureData loadPoleFigure

fid = fopen(fname,'r');
d = fread(fid);
fclose(fid);

try
  d(d==13) = [];
  d = char(d)';

  file_id = regexpi(d(1:200),'(?<=File ID:(.*?)\n)(.*?)\n','match');
  comment = file_id{1};
  h = string2Miller(comment);

  ranges = regexpsplit(d,'Range');
  axis = regexpsplit(ranges{1},'Axis');

  % extract the stepsize
  f = regexpi(axis(2:end),'(?<=Step Size  :)(.*?)\n','match');
  f = [f{:}]; f = [f{:}];
  f = str2num(f);
  step = f(find(f>0,1,'first'));

  % process the ranges
  for k=2:numel(ranges)
    range = ranges{k};

    po = sscanf(range,'%d')*step;

    low = sscanf(range(41:49),'%f');
    high = sscanf(range(87:95),'%f');

    bg = (low+high)/2;

    t = find(double(range(97:end))==10,1,'first');
    f = numel(sscanf(range(97:97+t),'%f'));

    data = sscanf(range(97:end),'%f');

    if f==3 % counts per second
      az = data(1:3:end);
      I(:,k-1) = data(2:3:end);
    else    % counts in time
      az = data(1:4:end);
      I(:,k-1) = data(2:4:end)*1000./data(3:4:end);
    end

    % background correction
    I(:,k-1) = I(:,k-1) - bg;

    % specimen direction
    r(:,k-1) = vector3d('theta',po*degree,'rho',az*degree);

  end

  pf = PoleFigure( h,r,I,varargin{:});
catch
  interfaceError(fname);
end


%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
function amtriant
%AMTRIANT Unit test for the function AMTRIANG.

%	O. Lemoine - February 1996.

N=256; t0=149; T=50;
sig=amtriang(N,t0,T);
if abs(sig(t0)-1)>sqrt(eps),				% sig(t0)=1
 error('amtriang test 1 failed ');
end
[tm,T1]=loctime(sig);
if abs(T-T1)>=sqrt(1/N),				% width
 error('amtriang test 3 failed ');
end
dist=1:min([N-t0,t0-1]);
if any(abs(sig(t0-dist)-sig(t0+dist))>sqrt(eps))~=0, 	% symmetry
 error('amtriang test 3 failed ');
end

N=12; t0=5; T=7;
sig=amtriang(N,t0,T);
if abs(sig(t0)-1)>sqrt(eps),				% sig(t0)=1
 error('amtriang test 4 failed ');
end
[tm,T1]=loctime(sig);
if abs(T-T1)>=sqrt(1/N),				% width
 error('amtriang test 5 failed ');
end
dist=1:min([N-t0,t0-1]);
if any(abs(sig(t0-dist)-sig(t0+dist))>sqrt(eps))~=0, 	% symmetry
 error('amtriang test 6 failed ');
end

N=535; t0=354; T=101;
sig=amtriang(N,t0,T);
if abs(sig(t0)-1)>sqrt(eps),				% sig(t0)=1
 error('amtriang test 7 failed ');
end
[tm,T1]=loctime(sig);
if abs(T-T1)>=sqrt(1/N),				% width
 error('amtriang test 8 failed ');
end
dist=1:min([N-t0,t0-1]);
if any(abs(sig(t0-dist)-sig(t0+dist))>sqrt(eps))~=0, 	% symmetry
 error('amtriang test 9 failed ');
end


function [dStates, contactForces] = dynamics_doubleStance(States, Actuators, Parameters)
% function [dStates, Actuators] = dynamics_doubleStance(States, Actuators, Parameters)
%
% Computer Generated File -- DO NOT EDIT
%
% This function was created by the function Write_ContinuousDynamics()
% 10-Dec-2013 19:40:47
%
% Dymanics Model: retractable double pendulum biped
% Motion Phase: Double Stance
%
% Matthew Kelly
% Cornell University
%

x = States(1,:); % (m) Foot One horizontal position
y = States(2,:); % (m) Foot One vertical position
th1 = States(3,:); % (rad) Leg One absolute angle
th2 = States(4,:); % (rad) Leg Two absolute angle
L1 = States(5,:); % (m) Leg One length
L2 = States(6,:); % (m) Leg Two length
dx = States(7,:); % (m/s) Foot One horizontal velocity
dy = States(8,:); % (m/s) Foot One vertical velocity
dth1 = States(9,:); % (rad/s) Leg One absolute angular rate
dth2 = States(10,:); % (rad/s) Leg Two absolute angular rate
dL1 = States(11,:); % (m/s) Leg One extension rate
dL2 = States(12,:); % (m/s) Leg Two extensioin rate

F1 = Actuators(1,:); % (N) Compresive axial force in Leg One
F2 = Actuators(2,:); % (N) Compresive axial force in Leg Two
T1 = Actuators(3,:); % (Nm) External torque applied to Leg One
T2 = Actuators(4,:); % (Nm) External torque applied to Leg Two
Thip = Actuators(5,:); % (Nm) Torque acting on Leg Two from Leg One

m1 = Parameters.m1; % (kg) Foot One mass
m2 = Parameters.m2; % (kg) Foot Two mass
M = Parameters.M; % (kg) Hip mass
g = Parameters.g; % (m/s^2) Gravity

% Constraints for this phase:
ddx = 0; %(m) Foot One horizontal position
ddy = 0; %(m) Foot One vertical position

dStates = zeros(size(States));
dStates(1:6,:) = States((1+6):(6+6),:);
dStates(7,:) = zeros(1,size(States,2));
dStates(8,:) = zeros(1,size(States,2));
dStates(9,:) = -(L2.*Thip - L2.*T1 + L1.*T2.*cos(th1 - th2) + L1.*Thip.*cos(th1 - th2) - F2.*L1.*L2.*sin(th1 - th2) + 2.*L1.*L2.*M.*dL1.*dth1 + L1.*L2.*M.*ddy.*cos(th1) + L1.*L2.*M.*g.*cos(th1) - L1.*L2.*M.*ddx.*sin(th1))./(L1.^2.*L2.*M);
dStates(10,:) = (L2.*Thip.*cos(th1).*cos(th2) - L2.*T1.*cos(th1).*cos(th2) - L2.*T1.*sin(th1).*sin(th2) + L2.*Thip.*sin(th1).*sin(th2) - L1.*L2.*M.*ddy.*cos(th2) + L1.*L2.*M.*ddx.*sin(th2) + L1.*T2.*cos(th1 - th2).*cos(th1).*cos(th2) + L1.*Thip.*cos(th1 - th2).*cos(th1).*cos(th2) + L1.*T2.*cos(th1 - th2).*sin(th1).*sin(th2) - L1.*T2.*sin(th1 - th2).*cos(th1).*sin(th2) + L1.*T2.*sin(th1 - th2).*cos(th2).*sin(th1) + L1.*Thip.*cos(th1 - th2).*sin(th1).*sin(th2) - L1.*Thip.*sin(th1 - th2).*cos(th1).*sin(th2) + L1.*Thip.*sin(th1 - th2).*cos(th2).*sin(th1) + F1.*L1.*L2.*cos(th1).*sin(th2) - F1.*L1.*L2.*cos(th2).*sin(th1) - F2.*L1.*L2.*cos(th1 - th2).*cos(th1).*sin(th2) + F2.*L1.*L2.*cos(th1 - th2).*cos(th2).*sin(th1) - F2.*L1.*L2.*sin(th1 - th2).*cos(th1).*cos(th2) - F2.*L1.*L2.*sin(th1 - th2).*sin(th1).*sin(th2) - 2.*L1.*L2.*M.*dL2.*dth2.*cos(th2).^2 - 2.*L1.*L2.*M.*dL2.*dth2.*sin(th2).^2 + L1.*L2.*M.*ddy.*cos(th1).^2.*cos(th2) + L1.*L2.*M.*g.*cos(th1).^2.*cos(th2) - L1.*L2.*M.*ddx.*cos(th1).^2.*sin(th2) + L1.*L2.*M.*ddy.*cos(th2).*sin(th1).^2 + L1.*L2.*M.*g.*cos(th2).*sin(th1).^2 - L1.*L2.*M.*ddx.*sin(th1).^2.*sin(th2))./(L1.*L2.^2.*M.*(cos(th2).^2 + sin(th2).^2));
dStates(11,:) = -(T2.*sin(th1 - th2) + Thip.*sin(th1 - th2) - F1.*L2 + F2.*L2.*cos(th1 - th2) - L1.*L2.*M.*dth1.^2 + L2.*M.*ddx.*cos(th1) + L2.*M.*ddy.*sin(th1) + L2.*M.*g.*sin(th1))./(L2.*M);
dStates(12,:) = (L2.*T1.*cos(th2).*sin(th1) - L2.*T1.*cos(th1).*sin(th2) + L2.*Thip.*cos(th1).*sin(th2) - L2.*Thip.*cos(th2).*sin(th1) - F1.*L1.*L2.*sin(th1).*sin(th2) - L1.*L2.*M.*ddx.*cos(th2) - L1.*L2.*M.*ddy.*sin(th2) + L1.*L2.^2.*M.*dth2.^2.*cos(th2).^2 + L1.*L2.^2.*M.*dth2.^2.*sin(th2).^2 + L1.*T2.*cos(th1 - th2).*cos(th1).*sin(th2) - L1.*T2.*cos(th1 - th2).*cos(th2).*sin(th1) + L1.*T2.*sin(th1 - th2).*cos(th1).*cos(th2) + L1.*Thip.*cos(th1 - th2).*cos(th1).*sin(th2) - L1.*Thip.*cos(th1 - th2).*cos(th2).*sin(th1) + L1.*Thip.*sin(th1 - th2).*cos(th1).*cos(th2) + L1.*T2.*sin(th1 - th2).*sin(th1).*sin(th2) + L1.*Thip.*sin(th1 - th2).*sin(th1).*sin(th2) - F1.*L1.*L2.*cos(th1).*cos(th2) + F2.*L1.*L2.*cos(th1 - th2).*sin(th1).*sin(th2) - F2.*L1.*L2.*sin(th1 - th2).*cos(th1).*sin(th2) + F2.*L1.*L2.*sin(th1 - th2).*cos(th2).*sin(th1) + L1.*L2.*M.*ddx.*cos(th1).^2.*cos(th2) + L1.*L2.*M.*ddx.*cos(th2).*sin(th1).^2 + L1.*L2.*M.*ddy.*cos(th1).^2.*sin(th2) + L1.*L2.*M.*g.*cos(th1).^2.*sin(th2) + L1.*L2.*M.*ddy.*sin(th1).^2.*sin(th2) + L1.*L2.*M.*g.*sin(th1).^2.*sin(th2) + F2.*L1.*L2.*cos(th1 - th2).*cos(th1).*cos(th2))./(L1.*L2.*M.*(cos(th2).^2 + sin(th2).^2));

% contactForces(1,:) == H1 == (N) Foot One, horizontal contact force
% contactForces(2,:) == V1 == (N) Foot One, vertical contact force
% contactForces(3,:) == H2 == (N) Foot Two, horizontal contact force
% contactForces(4,:) == V2 == (N) Foot Two, vertical contact force
contactForces = zeros(4,size(States,2));
contactForces(1,:) = (Thip.*sin(th1) - T1.*sin(th1) + L1.*ddx.*m1 + F1.*L1.*cos(th1))./L1;
contactForces(2,:) = (T1.*cos(th1) - Thip.*cos(th1) + L1.*ddy.*m1 + L1.*g.*m1 + F1.*L1.*sin(th1))./L1;
contactForces(3,:) = (L1.*M.*T2.*sin(th2) + L1.*M.*Thip.*sin(th2) + L2.*T1.*m2.*sin(th1) - L2.*Thip.*m2.*sin(th1) - L1.*L2.*M.*ddx.*m2 - L1.*T2.*m2.*cos(th1 - th2).*sin(th1) + L1.*T2.*m2.*sin(th1 - th2).*cos(th1) - L2.*T1.*m2.*cos(th1 - th2).*sin(th2) - L2.*T1.*m2.*sin(th1 - th2).*cos(th2) - L1.*Thip.*m2.*cos(th1 - th2).*sin(th1) + L1.*Thip.*m2.*sin(th1 - th2).*cos(th1) + L2.*Thip.*m2.*cos(th1 - th2).*sin(th2) + L2.*Thip.*m2.*sin(th1 - th2).*cos(th2) - F2.*L1.*L2.*M.*cos(th2) - F1.*L1.*L2.*m2.*cos(th1) + L1.*T2.*m2.*cos(th1 - th2).^2.*sin(th2) + L1.*Thip.*m2.*cos(th1 - th2).^2.*sin(th2) + L1.*T2.*m2.*sin(th1 - th2).^2.*sin(th2) + L1.*Thip.*m2.*sin(th1 - th2).^2.*sin(th2) + L1.*L2.*M.*ddx.*m2.*cos(th1).^2 + L1.*L2.*M.*ddx.*m2.*cos(th2).^2 + L1.*L2.*M.*ddx.*m2.*sin(th1).^2 + L1.*L2.*M.*ddx.*m2.*sin(th2).^2 + F1.*L1.*L2.*m2.*cos(th1 - th2).*cos(th2) + F2.*L1.*L2.*m2.*cos(th1 - th2).*cos(th1) - F1.*L1.*L2.*m2.*sin(th1 - th2).*sin(th2) + F2.*L1.*L2.*m2.*sin(th1 - th2).*sin(th1) - F2.*L1.*L2.*m2.*cos(th1 - th2).^2.*cos(th2) - F2.*L1.*L2.*m2.*sin(th1 - th2).^2.*cos(th2) + L1.*L2.*M.*ddy.*m2.*sin(th1 - th2).*sin(th1).*sin(th2) + L1.*L2.*M.*g.*m2.*sin(th1 - th2).*sin(th1).*sin(th2) - L1.*L2.*M.*ddx.*m2.*cos(th1 - th2).*cos(th1).*cos(th2) + L1.*L2.*M.*ddy.*m2.*cos(th1 - th2).*cos(th1).*sin(th2) - L1.*L2.*M.*ddy.*m2.*cos(th1 - th2).*cos(th2).*sin(th1) + L1.*L2.*M.*ddy.*m2.*sin(th1 - th2).*cos(th1).*cos(th2) + L1.*L2.*M.*g.*m2.*cos(th1 - th2).*cos(th1).*sin(th2) - L1.*L2.*M.*g.*m2.*cos(th1 - th2).*cos(th2).*sin(th1) + L1.*L2.*M.*g.*m2.*sin(th1 - th2).*cos(th1).*cos(th2) - L1.*L2.*M.*ddx.*m2.*cos(th1 - th2).*sin(th1).*sin(th2) + L1.*L2.*M.*ddx.*m2.*sin(th1 - th2).*cos(th1).*sin(th2) - L1.*L2.*M.*ddx.*m2.*sin(th1 - th2).*cos(th2).*sin(th1))./(L1.*L2.*M.*(cos(th2).^2 + sin(th2).^2));
contactForces(4,:) = -(L1.*M.*T2.*cos(th2) + L1.*M.*Thip.*cos(th2) + L2.*T1.*m2.*cos(th1) - L2.*Thip.*m2.*cos(th1) + L1.*L2.*M.*ddy.*m2 - L1.*T2.*m2.*cos(th1 - th2).*cos(th1) - L2.*T1.*m2.*cos(th1 - th2).*cos(th2) - L1.*Thip.*m2.*cos(th1 - th2).*cos(th1) + L2.*Thip.*m2.*cos(th1 - th2).*cos(th2) - L1.*T2.*m2.*sin(th1 - th2).*sin(th1) + L2.*T1.*m2.*sin(th1 - th2).*sin(th2) - L1.*Thip.*m2.*sin(th1 - th2).*sin(th1) - L2.*Thip.*m2.*sin(th1 - th2).*sin(th2) + F2.*L1.*L2.*M.*sin(th2) + F1.*L1.*L2.*m2.*sin(th1) + L1.*T2.*m2.*cos(th1 - th2).^2.*cos(th2) + L1.*Thip.*m2.*cos(th1 - th2).^2.*cos(th2) + L1.*T2.*m2.*sin(th1 - th2).^2.*cos(th2) + L1.*Thip.*m2.*sin(th1 - th2).^2.*cos(th2) + F2.*L1.*L2.*m2.*sin(th1 - th2).^2.*sin(th2) - L1.*L2.*M.*ddy.*m2.*cos(th1).^2 - L1.*L2.*M.*ddy.*m2.*cos(th2).^2 - L1.*L2.*M.*g.*m2.*cos(th1).^2 - L1.*L2.*M.*g.*m2.*cos(th2).^2 - L1.*L2.*M.*ddy.*m2.*sin(th1).^2 - L1.*L2.*M.*ddy.*m2.*sin(th2).^2 - L1.*L2.*M.*g.*m2.*sin(th1).^2 - L1.*L2.*M.*g.*m2.*sin(th2).^2 - F1.*L1.*L2.*m2.*cos(th1 - th2).*sin(th2) - F1.*L1.*L2.*m2.*sin(th1 - th2).*cos(th2) - F2.*L1.*L2.*m2.*cos(th1 - th2).*sin(th1) + F2.*L1.*L2.*m2.*sin(th1 - th2).*cos(th1) + F2.*L1.*L2.*m2.*cos(th1 - th2).^2.*sin(th2) + L1.*L2.*M.*ddx.*m2.*sin(th1 - th2).*sin(th1).*sin(th2) + L1.*L2.*M.*ddy.*m2.*cos(th1 - th2).*cos(th1).*cos(th2) + L1.*L2.*M.*g.*m2.*cos(th1 - th2).*cos(th1).*cos(th2) + L1.*L2.*M.*ddx.*m2.*cos(th1 - th2).*cos(th1).*sin(th2) - L1.*L2.*M.*ddx.*m2.*cos(th1 - th2).*cos(th2).*sin(th1) + L1.*L2.*M.*ddx.*m2.*sin(th1 - th2).*cos(th1).*cos(th2) + L1.*L2.*M.*ddy.*m2.*cos(th1 - th2).*sin(th1).*sin(th2) - L1.*L2.*M.*ddy.*m2.*sin(th1 - th2).*cos(th1).*sin(th2) + L1.*L2.*M.*ddy.*m2.*sin(th1 - th2).*cos(th2).*sin(th1) + L1.*L2.*M.*g.*m2.*cos(th1 - th2).*sin(th1).*sin(th2) - L1.*L2.*M.*g.*m2.*sin(th1 - th2).*cos(th1).*sin(th2) + L1.*L2.*M.*g.*m2.*sin(th1 - th2).*cos(th2).*sin(th1))./(L1.*L2.*M.*(cos(th2).^2 + sin(th2).^2));

end


function  cmap = GetColormap(clrmap_name,numK)
numK = double(numK);
data_dir = GetCurrentDataDir();
load(fullfile(data_dir,'CustomColormaps.mat'));
if strcmp(clrmap_name,'hsv_new'),
    cmap64 = hsv_new;
    cmap = InterpColormap(cmap64,numK);
elseif strcmp(clrmap_name,'jet'),
    cmap = flipud(jet(numK));
elseif strcmp(clrmap_name,'greedy_hsv'),
%     cmap64 = greedy_hsv;
%     cmap = InterpColormap(cmap64,numK);
% choose random colors!
%  cmap64 = greedy_hsv;
rng(1);
H = horzcat(rand(numK,1),rand(numK,1),0.5+0.5*rand(numK,1));
cmap = hsv2rgb(H);

elseif strcmp(clrmap_name,'hsv_old'),
    cmap = hsv(round(numK*1.1));
%     cmap64 = hsv_old;
% %     same as:
%     temp = hsv(70);
%     cmap64 = temp(1:64,:);

else
    cmap64 = hsv_new;
    cmap = InterpColormap(cmap64,numK);
end

% cmap = [1,0.956763267517090,0.00606909440830350;0.393025994300842,1,0.00697167776525021;0,1,1;0.402252703905106,0.00566019536927342,1;1,0,0;1,0.477354049682617,0.00717086857184768];

cmap(cmap<0) = 0;
end

function cmap = InterpColormap(cmap64,numK) % this doesn't work well for very small numK - too close to edge
k = double(numK);
c1 = interp1(1:64,cmap64(:,1),linspace(1,64,k),'pchip');
c2 = interp1(1:64,cmap64(:,2),linspace(1,64,k),'pchip');
c3 = interp1(1:64,cmap64(:,3),linspace(1,64,k),'pchip');
% c1 = interp1(1:64,cmap64(:,1),linspace(1,64,k),'spline');
% c2 = interp1(1:64,cmap64(:,2),linspace(1,64,k),'spline');
% c3 = interp1(1:64,cmap64(:,3),linspace(1,64,k),'spline');
cmap = [c1',c2',c3'];
end


function test_ipopt

  auxdata = {} ;

  options.lb = [ -Inf, -Inf ] ;  % Lower bound on the variables.
  options.ub = [  Inf, Inf ] ;  % Upper bound on the variables.

  % The constraint functions are bounded to zero
  options.cl = [ 0, 0, -Inf ]; %  constraints
  options.cu = [ Inf, Inf, 0];

  % Set up the auxiliary data.
  options.auxdata = auxdata ;

  % Set the IPOPT options.
  options.ipopt.jac_d_constant   = 'no';
  options.ipopt.hessian_constant = 'no';
  options.ipopt.mu_strategy      = 'adaptive';
  options.ipopt.max_iter         = 400;
  options.ipopt.tol              = 1e-10;
  options.ipopt.linear_solver = 'ma57';

  % The callback functions.
  funcs.objective         = @objective;
  funcs.constraints       = @constraints;
  funcs.gradient          = @gradient;
  funcs.jacobian          = @jacobian;
  funcs.jacobianstructure = @jacobianstructure;
  if false
    options.ipopt.derivative_test = 'first-order';
    funcs.hessian           = @hessian;
    funcs.hessianstructure  = @hessianstructure;
  else
    options.ipopt.hessian_approximation      = 'limited-memory';
    %options.ipopt.limited_memory_update_type = 'bfgs' ; % {bfgs}, sr1 = 6; % {6}
    %options.ipopt.limited_memory_update_type = 'sr1' ;
    options.ipopt.limited_memory_update_type = 'bfgs' ; % {bfgs}, sr1 = 6; % {6}
  end

  % Run IPOPT.
  x0 = [-2, 1] ;

  tic
  [x, info] = ipopt_auxdata(x0,funcs,options);
  elapsed = toc ;

  info;

  x

end

%%
% map the indices with the corresponding index in the spase matrix
function f = objective(x,auxdata)
  f = 100*(x(2)-x(1)^2)^2+(1-x(1))^2 ;
end

%%
% map the indices with the corresponding index in the spase matrix
function g = gradient(x,auxdata)
  g = [ 400*x(1)*(x(1)^2-x(2))+2*x(1)-2, ...
        200*(x(2)-x(1)^2) ] ;
end

function f = constraints(x,auxdata)
  f = zeros(3,1) ;
  f(1) = x(1)*x(2)-1 ; % = 0
  f(2) = x(1) + x(2)^2 ; % >= 0
  f(3) = x(1) ;
end

function jac = jacobian(x,auxdata)
  jac = sparse([ x(2), x(1)   ; ...
                 1,    2*x(2) ; ...
                 1,    0 ]) ;
end

function jac = jacobianstructure(auxdata)
  jac = sparse(ones(3,2)) ;
end

function H = hessian(x, sigma, lambda, auxdata)
  H = sigma * [ 1200*x(1)^2-400*x(2)+2, 0 ; ...
                 -400*x(1)               200] ;
  H = H + lambda(1) * [ 0 0 ; 1 0 ] ;
  H = H + lambda(2) * [ 0 0 ; 0 2 ] ;
  H = sparse(H) ;
end

function H = hessianstructure(auxdata)
  H = sparse([ 1 0 ; 1 1]) ;
end


%   AUTHORSHIP
%   Primary Developer: Stephen Meehan <swmeehan@stanford.edu>
%   Bioinformatics Lead:  Wayne Moore <wmoore@stanford.edu>
%   Provided by the Herzenberg Lab at Stanford University
%   License: BSD 3 clause
%
function [x, y]=clockwise(xy)
x=xy(:,1);
y=xy(:,2);
cx = mean(x);
cy = mean(y);
a = atan2(y - cy, x - cx);
[~, order] = sort(a);
x = x(order);
y = y(order);
%assert(all(ismember([x y],xy,'rows')));
end


function P = P_given_TVN(x)
% Downloaded from the personal page of Professor Sigurd Skogestad
% http://www.nt.ntnu.no/users/skoge/diplom/prosjekt03/wold/MatLab/
A = co2_sw(x);
P = -A.g(2);


function F = uminus( F )
%-   Unary minus of a SPHEREFUNV
%   -F returns the SPHEREFUNV negated componentwise.
%
%   UMINUS(F) is called by the syntax '-F'.

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

% Empty check:
if (isempty(F) )
    return
end

% Take uminus of each component:
F.components = cellfun(@uminus, F.components, 'UniformOutput', false);

end


function [err,contrfile] = ContrastTest (N_runs, polort, N_basis, Task, N_tasks, N_contr, El, N_base)
%
%   [err,] = ContrastTest ()
%
%Purpose:
%   Construct the contrast test matrix used in -glt option of 3dDeconvolve.
%
%
%Input Parameters:
%
%
%
%Output Parameters:
%   err : 0 No Problem
%       : 1  Problems
%
%
%
%Key Terms:
%
%More Info :
%
%
%
%
%     Author : Gang Chen
%     Date : Tue Jul 29 13:01:05 EDT 2003
%     SSCC/NIMH/ National Institutes of Health, Bethesda Maryland

%Define the function name for easy referencing
FuncName = 'ContrastTest';

%Debug Flag
DBG = 1;

%initailize return variables
err = 1;

%Create a matrix so that 3dDeconvolve uses for regressor test.

length = (polort+1)*N_runs+N_base;
for (iT = 1:1:N_tasks),
   for (j = 1:1:N_basis)    ,
	   length = length + (Task(iT).BasisOpt_struct(j).maxlag - Task(iT).BasisOpt_struct(j).minlag + 1);
	end
end

%mtrx = zeros(N_contr, (polort+1)*N_runs+N_basis*N_tasks);  %for each run, there are (polort+1) paramters in the baseline model

mtrx = zeros(N_contr, length);
fprintf(1, '\n\nYour contrast vectors for -glt options are:\n');

for (i = 1:1:N_contr),
	for (j = 1:1:El(i).cnt),
	   for (k = 1:1:N_tasks),
		   if El(i).order(j) == k,
			   for (m=1:1:N_basis),
				   for (p = 1:1:Task(k).BasisOpt_struct(m).maxlag - Task(k).BasisOpt_struct(m).minlag + 1)
%			         mtrx(i,m+(k-1)*N_basis+(polort+1)*N_runs) = El(i).sign(j);
                  mtrx(i,p+(m-1)*(Task(k).BasisOpt_struct(m).maxlag - Task(k).BasisOpt_struct(m).minlag + 1)...
					      +(k-1)*(Task(k).BasisOpt_struct(m).maxlag - Task(k).BasisOpt_struct(m).minlag + 1)*N_basis...
					      +(polort+1)*N_runs) = El(i).sign(j);    %Don't ask me how I worked this out. It is so tedious and nasty
							                                        %to housekeep the indices.
							                                        %Don't even remember how I figured it out now
					end
				end
	      end
		end
	end
	contrfile(i).name = sprintf('%s_contr.1D',El(i).labels);
   fid = fopen(contrfile(i).name, 'w');
	fprintf(1,'No. %i contrast vector = ', i);
	for (n = 1:1:length),
		fprintf(1, '%g	', mtrx(i,n));
		fprintf(fid, '%g ', mtrx(i,n));
	end
	fprintf(1, '\n');
	fclose(fid);
	fprintf(1, '\nThe vector for No. %i contrast is stored in file %s. \n\n', i, contrfile(i).name);
end

err = 0;
return;


function ll = svargplvmLogLikelihood(model)

% SVARGPLVMLOGLIKELIHOOD Log-likelihood for a shared variational GP-LVM.
% FORMAT
% DESC returns the log likelihood for a given SVARGP-LVM model.
% ARG model : the model for which the log likelihood is to be
% computed. The model contains the data for which the likelihood is
% being computed in the 'y' component of the structure.
% RETURN ll : the log likelihood of the data given the model.
%
% SEEALSO:  svargplvmLogLikeGradients
%
% COPYRIGHT : Andreas C. Damianou, 2011

% VARGPLVM

% Functions f1 and f2 should be equivalent in the static case, but f1 might
% (?) be faster.
if ~isfield(model, 'dynamics') || isempty(model.dynamics)
    ll = f1(model);
    %ll = f2(model);
else
    ll = f2(model);
    for i = 1:model.numModels
        if isfield(model.comp{i}, 'KLweight')
            assert(model.comp{i}.KLweight == 0.5); % not implemented yet for dynamics
        end
    end
end

function f = f1(model)

% model.numModels=1; %%%%%%%%TEMP

% This works only when there are NOT any dynamics:
varmeans = sum(sum(model.vardist.means.*model.vardist.means));
varcovs = sum(sum(model.vardist.covars - log(model.vardist.covars)));
KLdiv = -0.5*(varmeans + varcovs) + 0.5*model.q*model.N;

model = svargplvmPropagateField(model, 'onlyLikelihood', 1);
ll=0;
for i=1:model.numModels
    ll = ll + vargplvmLogLikelihood(model.comp{i});
end
f = (ll + KLdiv);

% Here we take advantage of the fact that in the bound, the likelihood and
% the KL part break into ll+KL. So, in the shared case, we only have
% KL + ll1 + ll2 + ...
function f = f2(model)

% We want to count for the KL part only once, because it is shared. Then, we
% will add all likelihood parts from all sub-models. The above can be
% achieved by calculating the bound for the first sub-model as usual, and
% then calculate only the likelihood part for the rest of the sub-models
% (because the KL part would be the same for all models since we called
% expandParam previously).
f = vargplvmLogLikelihood(model.comp{1}); % ll_1 + KL_1 (where KL_1 == KL_i for all i)
% The following call will not affect the model after returning from this
% function, since the model is not returned.
model = svargplvmPropagateField(model, 'onlyLikelihood', 1, true);
ll=0;
for i=2:model.numModels
    ll = ll + vargplvmLogLikelihood(model.comp{i}); % ll_i
end

f = f + ll;


function im = VisualizeClassifier(jabfile)

%% params.
params = getParams;
npatches = params.npatches;
psize = params.psize;
nbins = params.nbins;
patchsz = params.patchsz;
scale = params.scale;

wd = params.wd;

%% load an example fly.

Q = load(jabfile,'-mat');

bdir = Q.x.expDirNames{1};
moviename = fullfile(bdir,'movie.ufmf');
trackfilename = fullfile(bdir,'trx.mat');
tracks = load(trackfilename);
tracks = tracks.trx;

[readfcn,nframes,fid,headerinfo] = get_readframe_fcn(moviename);
trackndx = 1; fly = 1;
fnum = tracks(fly).firstframe;
im1 = readfcn(fnum);

locy = round(tracks(fly).y(trackndx));
locx = round(tracks(fly).x(trackndx));
im1 = extractPatch(im1,...
  locy,locx,tracks(fly).theta(trackndx),patchsz);

im1 =[im1 im1];

%% Find Hog/HOF components.

ncl = numel(Q.x.classifierStuff);

for clnum = 1:ncl

  H{clnum} = zeros(npatches,npatches,nbins);
  F{clnum} = H{clnum};

  cl = Q.x.classifierStuff(clnum).params;
  fnames = Q.x.classifierStuff(clnum).featureNames;
  for ndx = 1:numel(cl)
    curf = fnames{cl(ndx).dim}{1};
    val = cl(ndx).alpha;
    prts = strsplit(curf,'_');
    loc = str2double(prts(end-2:end));
    if strcmp(curf(1:2),'hf')
      H{clnum}(loc(1),loc(2),loc(3))=H{clnum}(loc(1),loc(2),loc(3))+val;
    else
      F{clnum}(loc(1),loc(2),loc(3))=F{clnum}(loc(1),loc(2),loc(3))+val;
    end
  end
end
%% Draw them

% For HOG
bincenters = linspace(0,pi,nbins+1);
bincenters = bincenters(1:nbins);
dt = mean(diff(bincenters));
binedges = [bincenters(1)-dt/2,(bincenters(1:end-1)+bincenters(2:end))/2,bincenters(end)+dt/2];

% For HOF
bincenters2 = bincenters*2;
dt = mean(diff(bincenters2));
binedges2 = [bincenters2(1)-dt/2,(bincenters2(1:end-1)+bincenters2(2:end))/2,bincenters2(end)+dt/2];

maxv = 3;
im = {};
for clnum = 1:ncl
  hfig = figure;
  clf;
  hax = axes('Position',[0,0,1,1]);
  set(hfig,'Units','pixels','Position',get(0,'ScreenSize'));

  im1curr = im1;

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

  colors = hsv(nbins);

  [nr,nc,~] = size(im1);
  nc = nc/2;

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

      for bini = 1:nbins,
        tmp = linspace(binedges2(bini),binedges2(bini+1),20);
        xcurr = cx + [0,psize/2*cos(tmp),0]*scale;
        ycurr = cy + [0,psize/2*sin(tmp),0]*scale;
        h(yi,xi,bini) = patch(xcurr,ycurr,colors(bini,:),'LineStyle','none','FaceAlpha',min(1,F{clnum}(yi,xi,bini)/maxv));
      end

    end
  end

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

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

    end
  end
  im{clnum} = getframe(hax);
end