function demoImOrientedBox(varargin)
%DEMOIMORIENTEDBOX Demo file for using function imOrientedBox
%
%   Syntax
%   demoImOrientedBox
%   The demo runs automatically. A basic segmentation is performed on a
%   demo image, then oriented boxes are computed for each particle and
%   displayed on the original image.
%
%   Example
%   demoImOrientedBox
%
%   See also
%   demoImMaxFeretDiameter, imOrientedBox

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-05-09,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

%% Image segmentation

% read image
img = imread('rice.png');
figure(1); clf;
imshow(img);

% compute background
bg = imopen(img, ones(30, 30));
img2 = img - bg;
figure(2); clf;
imshow(img2);

% display histogram, to identify the threshold value
figure(3); clf;
imHistogram(img2);

% image binarisation, and remove particles touching border
bin = img2 > 50;
bin = imclearborder(bin, 4);
imshow(bin);

% compute image labels, using minimal connectivity
lbl = bwlabel(bin, 4);
nLabels = max(lbl(:));

% display label image
rgb = label2rgb(lbl, jet(nLabels), 'w', 'shuffle');
figure(4); clf;
imshow(rgb);

%% Compute enclosing oriented boxes

% call the function
boxes = imOrientedBox(lbl);

% display result
hold on;
drawOrientedBox(boxes, 'linewidth', 2);


function splabels = msLabelMap2Sp(lmap, smap)

nocell = false;
if ~iscell(lmap)
    nocell = true;
    lmap = {lmap};
    smap = {smap};
end

% if ~exist('maxlab', 'var') || isempty(maxlab)
%     maxlab = 0;
%     for f = 1:numel(lmap)
%         maxlab = max(maxlab, max(lmap{f}(:)));
%     end
% end

for f = 1:numel(lmap)

    nseg = max(smap{f}(:));
    stats = regionprops(smap{f}, 'PixelIdxList');
    idx = {stats.PixelIdxList};

    splabels{f} = zeros(nseg, 1);
    for s = 1:nseg
        splabels{f}(s) = mode(double(lmap{f}(idx{s})));
    end
end

if nocell
    splabels = splabels{1};
end


function field=meg_forward(dip_par,forwpar)
% calculates the magnetic field of n dipoles
% in a realistic volume conductor
% usage: field=meg_forward(dip_par,forwpar)
%
% input:
% dip_par nx6 matrix where each row contains location (first 3 numbers)
%         and moment (second 3 numbers) of a dipole
% forwpar structure containing all information relevant for this
%         calculation; forwpar is calculated with meg_ini
%         You have here an option to include linear transformations in
%         the forward model by specifying forpwar.lintrafo=A
%         where A is an NxM matrix. Then field -> A field
%         You can use that, e.g., if you can write the forward model
%         with M magnetometer-channels plus a matrix multiplication
%         transforming this to a (eventually higher order) gradiometer.
%
% output:
% field  mxn matrix where the i.th column is the field in m channels
%        of the i.th dipole
%
% note:  No assumptions about units are made (i.e. no scaling factors)
%
% Copyright (C) 2003, Guido Nolte
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

device_sens=forwpar.device_sens;

field_sens_sphere=getfield_sphere(dip_par,forwpar.device_sens,forwpar.center);
field=field_sens_sphere;clear field_sens_sphere;
if isfield(forwpar,'device_ref')
  field=field-getfield_sphere(dip_par,forwpar.device_ref,forwpar.center);
end
if isfield(forwpar,'device_weights')
  field=field+forwpar.weights*getfield_sphere(dip_par,forwpar.device_weights,forwpar.center);
end

if forwpar.order>0
  coeff=forwpar.coeff_sens;
  if isfield(forwpar,'device_ref')
    coeff=coeff-forwpar.coeff_ref;
  end
  if isfield(forwpar,'device_weights')
    coeff=coeff+forwpar.coeff_weights*forwpar.weights';
  end
  field=field+getfield_corr(dip_par,coeff,forwpar.center,forwpar.order);
end

if isfield(forwpar,'lintrafo');
  field=forwpar.lintrafo*field;
end

return % main function

function field=getfield_sphere(source,device,center)
[ndip,ndum]=size(source);
[nchan,ndum]=size(device);
x1=source(:,1:3)-repmat(center',ndip,1);
n1=source(:,4:6);
x2=device(:,1:3)-repmat(center',nchan,1);
n2=device(:,4:6);
%spherical
bt=leadsphere_all(x1',x2',n2');
n1rep=reshape(repmat(n1',1,nchan),3,ndip,nchan);
b=dotproduct(n1rep,bt);
field=b';
return

function field=getfield_corr(source,coeffs,center,order)
[ndip,ndum]=size(source);
x1=source(:,1:3)-repmat(center',ndip,1);
n1=source(:,4:6);
%correction
if order>0
  scale=10;
  [bas,gradbas]=legs(x1,n1,order,scale);
  nbasis=(order+1)^2-1;
  coeffs=coeffs(1:nbasis,:);
  field=-(gradbas*coeffs)';
end
return

function out=crossproduct(x,y)
% usage: out=testprog(x,y)
% testprog calculates the cross-product of vector x and y
[n,m,k]=size(x);
out=zeros(3,m,k);
out(1,:,:)=x(2,:,:).*y(3,:,:)-x(3,:,:).*y(2,:,:);
out(2,:,:)=x(3,:,:).*y(1,:,:)-x(1,:,:).*y(3,:,:);
out(3,:,:)=x(1,:,:).*y(2,:,:)-x(2,:,:).*y(1,:,:);
return

function out=dotproduct(x,y)
% usage: out=dotproduct(x,y)
% testprog calculates the dotproduct of vector x and y
[n,m,k]=size(x);
outb=x(1,:,:).*y(1,:,:)+x(2,:,:).*y(2,:,:)+x(3,:,:).*y(3,:,:);
out=reshape(outb,m,k);
return

function result=norms(x)
[n,m,k]=size(x);
resultb=sqrt(x(1,:,:).^2+x(2,:,:).^2+x(3,:,:).^2);
result=reshape(resultb,m,k);
return


% averef() - convert common-reference EEG data to average reference
%            Note that this old function is not being used in EEGLAB. The
%            function used by EEGLAB is reref().
%
% Usage:
%   >> data = averef(data);
%   >> [data_out W_out S_out meandata] = averef(data,W);
%
% Inputs:
%   data - 2D data matrix (chans,frames*epochs)
%   W    - ICA weight matrix
%
% Outputs:
%   data_out - Input data converted to average reference.
%   W_out    - ICA weight matrix converted to average reference
%   S_out    - ICA sphere matrix converted to eye()
%   meandata - (1,dataframes) mean removed from each data frame (point)
%
% Note: If 2 args, also converts the weight matrix W to average reference:
%         If ica_act = W*data, then data = inv(W)*ica_act;
%         If R*data is the average-referenced data,
%         R*data=(R*inv(W))*ica_act and W_out = inv(R*inv(W));
%         The average-reference ICA maps are the columns of inv(W_out).
%
% Authors: Scott Makeig and Arnaud Delorme, SCCN/INC/UCSD, La Jolla, 1999
%
% See also: reref()

% Copyright (C) 1999 Scott Makeig, SCCN/INC/UCSD, scott@sccn.ucsd.edu
%
% 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% 12/16/99 Corrected denomiator on the suggestion of Ian Nimmo-Smith, Cambridge UK
% 01-25-02 reformated help & license -ad

function [data, W, S, meandata] = averef(data, W, S)

if nargin<1
  help averef
  return
end
chans = size(data,1);
if chans < 2
  help averef
  return
end

% avematrix = eye(chans)-ones(chans)*1/chans;
% data = avematrix*data; % implement as a matrix multiply
% else (faster?)

meandata = sum(data)/chans;
data = data - ones(chans,1)*meandata;

% treat optional ica parameters
if nargin == 2
	winv  = pinv(W);
    size1 = size(winv,1);
	avematrix = eye(size1)-ones(size1)*1/size1;
	W = pinv(avematrix*winv);
end;
if nargin >= 3
	winv = pinv(W*S);
    size1 = size(winv,1);
	avematrix = eye(size1)-ones(size1)*1/size1;
	W = pinv(avematrix*winv);
	S = eye(chans);
end;


function T45=T45(h)
T45=2.78e-3*h+139.1;


function [dat] = read_nexstim_nxe(filename, begsample, endsample, chanindx)

% READ_NEXSTIM_NXE reads specified samples from a NXE continuous datafile
%
% Use as
%   [hdr] = read_nexstim_nxe(filename)
% where
%    filename        name of the datafile, including the .bdf extension
% This returns a header structure with the following elements
%   hdr.Fs           sampling frequency
%   hdr.nChans       number of channels
%   hdr.nSamples     number of samples per trial
%   hdr.nSamplesPre  number of pre-trigger samples in each trial
%   hdr.nTrials      number of trials
%   hdr.label        cell-array with labels of each channel
%
% Or use as
%   [dat] = read_nexstim_nxe(filename, begsample, endsample, chanindx)
% where
%    filename        name of the datafile, including the .nxe extension
%    begsample       index of the first sample to read
%    endsample       index of the last sample to read
%    chanindx        index of channels to read (optional, default is all)
% This returns a Nchans X Nsamples data matrix

% Written by Vladimir Litvak based on functions provided by Nexstim
%
% Copyright (C) 2007, Vladimir Litvak
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if nargin==1
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % read the header
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    hdr.Fs          = 1450;
    hdr.nChans      = 64;
    hdr.label  = cell(64,1);
    hdr.label(1:4)= {'GATE', 'TRIG1', 'TRIG2','EOG'};
    hdr.label(5:64) = {
        'Fp1'
        'Fpz'
        'Fp2'
        'AF1'
        'AFz'
        'AF2'
        'F7'
        'F3'
        'F1'
        'Fz'
        'F2'
        'F4'
        'F8'
        'FT9'
        'FT7'
        'FC5'
        'FC3'
        'FC1'
        'FCz'
        'FC2'
        'FC4'
        'FC6'
        'FT8'
        'FT10'
        'T7'
        'C5'
        'C3'
        'C1'
        'Cz'
        'C2'
        'C4'
        'C6'
        'T8'
        'TP9'
        'TP7'
        'CP5'
        'CP3'
        'CP1'
        'CPz'
        'CP2'
        'CP4'
        'CP6'
        'TP8'
        'TP10'
        'P9'
        'P7'
        'P3'
        'P1'
        'Pz'
        'P2'
        'P4'
        'P8'
        'P10'
        'PO3'
        'POz'
        'PO4'
        'O1'
        'Oz'
        'O2'
        'Iz'};

    % it is continuous data, therefore append all records in one trial
    hdr.nTrials     = 1;

    fid=fopen_or_error(filename,'r','l');
    fseek(fid,0,'eof');
    numBytes = ftell(fid);
    hdr.nSamples = (numBytes/2)/hdr.nChans;

    hdr.nSamplesPre = 0;

    fclose(fid);

    % return the header
    dat = hdr;

else
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % read the data
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    sfEEG = (1/2000) * (10/65535) * 1000000;
    sfEOG = (1/400) * (10/65535) * 1000000;
    sfTRIG = 10 * (10/65535);

    numChannels = 64;

    fid = fopen_or_error(filename,'r','l');
    fseek(fid, 2*numChannels*(begsample-1),'bof');
    data = fread(fid,[numChannels endsample-begsample+1],'short');
    fclose(fid);

    data(1:3,:) = sfTRIG.*data(1:3,:);
    data(4,:) = sfEOG.*data(4,:);
    data(5:64,:) = sfEEG.*data(5:64,:);

    if nargin<4
        chanindx = 1:numChannels;
    end

    dat = data(chanindx,:);
end


function [ind,value] = cnn_classifier(A,dims,classifier,thr)

%cnn_classifer classify spatial components using a pretrained CNN
%classifier using the keras importer add on.
%   IND = cnn_classifier(A,dims,classifier,thr) returns a binary vector indicating
%   whether the set of spatial components A, with dimensions of the field
%   of view DIMS, pass the threshold THR for the given CLASSIFIER
%
%   [IND,VALUE] = cnn_classifier(A,dims,classifier,thr) also returns the
%   output value of the classifier
%
%   INPUTS:
%   A:              2d matrix
%   dims:           vector with dimensions of the FOV
%   classifier:     path to pretrained classifier model (downloaded if it
%                       doesn't exist)
%   thr:            threshold for accepting component (default: 0.2)
%
%   note: The function requires Matlab version 2017b (9.3) or later, Neural
%   Networks toolbox version 2017b (11.0) or later, the Neural Network
%   Toolbox(TM) Importer for TensorFlow-Keras Models.

%   Written by Eftychios A. Pnevmatikakis. Classifier trained by Andrea
%   Giovannucci, Flatiron Institute, 2017

K = size(A,2);                          % number of components

if verLessThan('matlab','9.3') || verLessThan('nnet','11.0') || isempty(which('importKerasNetwork'))
    warning(strcat('The function cnn_classifier requires Matlab version 2017b (9.3) or later, Neural\n', ...
        'Networks toolbox version 2017b (11.0) or later, the Neural Networks ', ...
        'Toolbox(TM) Importer for TensorFlow-Keras Models.'))
    ind = true(K,1);
    value = ones(K,1);
else
    if ~exist('thr','var'); thr = 0.2; end

    A = A/spdiags(sqrt(sum(A.^2,1))'+eps,0,K,K);      % normalize to sum 1 for each compoennt
    A_com = extract_patch(A,dims,[50,50]);  % extract 50 x 50 patches

    if ~exist(classifier,'file')
        url = 'https://www.dropbox.com/s/1csymylbne7yyt0/cnn_model.h5?dl=1';
        classifier = 'cnn_model.h5';
        outfilename = websave(classifier,url);
    end

    net_classifier = importKerasNetwork(classifier,'ClassNames',["rejected","accepted"]);
    out = predict(net_classifier,double(A_com));
    value = out(:,2);
    ind = (value >= thr);
end


% Demo for Structured Edge Detector (please see readme.txt first).
addpath(genpath('../piotr_toolbox'));

%% set opts for training (see edgesTrain.m)
opts=edgesTrain();                % default options (good settings)
opts.modelDir='models/';          % model will be in models/forest
opts.modelFnm='modelBsds';        % model name
opts.nPos=5e5; opts.nNeg=5e5;     % decrease to speedup training
opts.useParfor=0;                 % parallelize if sufficient memory

%% train edge detector (~20m/8Gb per tree, proportional to nPos/nNeg)
tic, model=edgesTrain(opts); toc; % will load model if already trained

%% set detection parameters (can set after training)
model.opts.multiscale=0;          % for top accuracy set multiscale=1
model.opts.sharpen=2;             % for top speed set sharpen=0
model.opts.nTreesEval=4;          % for top speed set nTreesEval=1
model.opts.nThreads=4;            % max number threads for evaluation
model.opts.nms=0;                 % set to true to enable nms

%% evaluate edge detector on BSDS500 (see edgesEval.m)
if(0), edgesEval( model, 'show',1, 'name','' ); end

%% detect edge and visualize results
I = imread('peppers.png');
tic, E=edgesDetect(I,model); toc
figure(1); im(I); figure(2); im(1-E);


function printv(v)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (C) 2020-2025, by Kai Chen, All rights reserved.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
n=size(v,1);
fprintf('v     =\n');
for i=1:n
    fprintf('%11.6f',v(i));
    fprintf('\n');
end
fprintf('nv=%d\n',n);

return


function bicycle_lock ( )

%*****************************************************************************80
%
%% BICYCLE_LOCK finds the combination on a typical bicycle lock.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 May 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
  timestamp ( )
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BICYCLE_LOCK\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  A bicycle combination lock consists of 3 dials,\n' );
  fprintf ( 1, '  each having 10 symbols, 0 through 9.\n' );
  fprintf ( 1, '  We seek to determine the combination C.\n' );
%
%  Set the combination randomly.
%  We can think of the combination as a number between 0 and 999.
%
  rng ( 'shuffle' );
  c = randi ( [ 0, 999 ], 1, 1 );
%
%  To find the combination, simply generate every number between 0 and 999,
%  and then try it.
%
  for a = 0 : 999

    if ( a == c )
      fprintf ( 1, '\n' );
      fprintf ( 1, '  The combination is %d!\n', c );
      break
    end

  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BICYCLE_LOCK\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end


function Cw = getCameraCenter(camRtC2W)

Cw = camRtC2W(1:3,4);


function [P,Uinit] = parafac_als(X,R,opts)
%PARAFAC_ALS Deprecated. Use CP_ALS instead.
%
%   See also CP_ALS.
%
%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 (nargout == 2) & (nargin == 3)
    [P,Uinit] = cp_als(X,R,opts);
elseif (nargout == 2) & (nargin == 2)
    [P,Uinit] = cp_als(X,R);
elseif (nargout == 2) & (nargin == 1)
    [P,Uinit] = cp_als(X);
elseif (nargout == 1) & (nargin == 3)
    P = cp_als(X,R,opts);
elseif (nargout == 1) & (nargin == 2)
    P = cp_als(X,R);
elseif (nargout == 1) & (nargin == 1)
    P = cp_als(X);
end


function varargout = squeeze(varargin)
%SQUEEZE   Squeeze a CHEBFUN2 to one variable, if possible.
%   G = squeeze(F) returns a CHEBFUN2 if F depends on x and y. If F depends only
%   on the x-variable a row CHEBFUN is returned and if it depends on just the
%   y-variable a column CHEBFUN is returned.

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

[varargout{1:nargout}] = squeeze@separableApprox(varargin{:});

end


function vout = Ptransp_solve(L, vin)
%Ptransp_solve  Auxi,ilary function for IR Tools
%
% vout = Ptransp_solve
%
% This computes vout = L'\vin.  If L is a function handle, then we use the
% user-supplied function, which is passed via options as 'RegMatrix'.
% Otherwise, we use the backslash operator.

% Silvia Gazzola, University of Bath
% Per Christian Hansen, Technical University of Denmark
% James G. Nagy, Emory University
% April, 2018.

% This file is part of the IR Tools package and is distributed under the
% 3-Clause BSD Licence. A separate license file should be provided as part
% of the package.

if isa(L, 'function_handle')
    transp_flag = 'transp';
    vout = L(vin, transp_flag);
else
    vout = (L')\vin;
end


function out = rdivide(T,S)

if isa(S,'double')

  if numel(S)>1
    S = repmat(reshape(S,[ones(1,T.rank) size(T)]), [3*ones(1,T.rank),1,1]);
  end

  T.M = T.M ./ S;
  out = T;

elseif isa(S,'tensor')

  T.M = T.M ./ S.M;
  out = T;

end


classdef AltitudeTermCondition < AbstractEventTerminationCondition
    %AltitudeTermCondition Summary of this class goes here
    %   Detailed explanation goes here

    properties
        altitude(1,1) double = 0; %km
        bodyInfo KSPTOT_BodyInfo
    end

    methods
        function obj = AltitudeTermCondition(altitude)
            obj.altitude = altitude;
        end

        function evtTermCondFcnHndl = getEventTermCondFuncHandle(obj)
            evtTermCondFcnHndl = @(t,y) obj.eventTermCond(t,y);
        end

        function initTermCondition(obj, initialStateLogEntry)
            obj.bodyInfo = initialStateLogEntry.centralBody;
        end

        function name = getName(obj)
            name = sprintf('Altitude (%.3f km)', obj.altitude);
        end

        function tf = shouldBeReinitOnRestart(obj)
            tf = true;
        end

        function params = getTermCondUiStruct(obj)
            params = struct();

            params.paramName = 'Altitude';
            params.paramUnit = 'km';
            params.useParam = 'on';
            params.useStages = 'off';
            params.useTanks = 'off';
            params.useEngines = 'off';
            params.useStopwatches = 'off';

            params.value = obj.altitude;
            params.refStage = LaunchVehicleStage.empty(1,0);
            params.refTank = LaunchVehicleEngine.empty(1,0);
            params.refEngine = LaunchVehicleEngine.empty(1,0);
            params.refStopwatch = LaunchVehicleStopwatch.empty(1,0);
        end

        function optVar = getNewOptVar(obj)
            optVar = AltitudeOptimizationVariable(obj);
        end

        function optVar = getExistingOptVar(obj)
            optVar = obj.optVar;
        end

        function tf = usesStage(obj, stage)
            tf = false;
        end

        function tf = usesEngine(obj, engine)
            tf = false;
        end

        function tf = usesTank(obj, tank)
            tf = false;
        end

        function tf = usesEngineToTankConn(obj, engineToTank)
            tf = false;
        end

        function tf = usesStopwatch(obj, stopwatch)
            tf = false;
        end
    end

    methods(Static)
        function termCond = getTermCondForParams(paramValue, stage, tank, engine, stopwatch)
            termCond = AltitudeTermCondition(paramValue);
        end
    end

    methods(Access=private)
        function [value,isterminal,direction] = eventTermCond(obj, t,y)
            rVect = y(1:3);
            vVect = y(4:6);
            cartElem = CartesianElementSet(t, rVect(:), vVect(:), obj.bodyInfo.getBodyCenteredInertialFrame());
            geoElem = cartElem.convertToFrame(obj.frame).convertToGeographicElementSet();

            actualAltitude = geoElem.alt;

            value = actualAltitude - obj.altitude;
            isterminal = 1;
            direction = 0;
        end
    end
end


function boundaryBenchGraphs(pbDir)
% function boundaryBenchGraphs(pbDir)
%
% Create graphs, after boundaryBench(pbDir) has been run.
%
% See also boundaryBench.
%
% David Martin <dmartin@eecs.berkeley.edu>
% May 2003

fname = fullfile(pbDir,'scores.txt');
scores = dlmread(fname); % iid,thresh,r,p,f
fname = fullfile(pbDir,'score.txt');
score = dlmread(fname); % thresh,r,p,f

% create the overall PR graph
fname = fullfile(pbDir,'pr.txt');
pr = dlmread(fname); % thresh,r,p,f
h = figure(1); clf; hold on;
prplot(h,pr(:,2),pr(:,3),sprintf('F=%4.2f',score(4)));
ph=plot(score(2),score(3),'ko','MarkerFaceColor','k','MarkerSize',10);
lh=legend(ph,sprintf('F=%4.2f @(%4.2f,%4.2f) t=%4.2f',...
                     score(4),score(2),score(3),score(1)));
p=get(lh,'Position');
p(1)=0.25;
p(2)=0.15;
set(lh,'Position',p);
print(h,'-depsc2',fullfile(pbDir,'pr.eps'));
print(h,'-djpeg95','-r36',fullfile(pbDir,'pr_half.jpg'));
print(h,'-djpeg95','-r0',fullfile(pbDir,'pr_full.jpg'));
%close(h);

iids = imgList('test');
for i = 1:numel(iids),
  iid = iids(i);
  fprintf(2,'Processing image %d/%d (iid=%d)...\n',i,numel(iids),iid);

  % create PR graphs for this image
  fname = fullfile(pbDir,sprintf('%d_pr.txt',iid));
  pri = dlmread(fname);
  h = figure(1); clf; hold on;
  prplot(h,pri(:,2),pri(:,3),sprintf('%d F=%4.2f',iid,scores(i,5)));
  ph=plot(scores(i,3),scores(i,4),'ko','MarkerFaceColor','k','MarkerSize',10);
  lh=legend(ph,sprintf('F=%4.2f @(%4.2f,%4.2f) t=%4.2f',...
                       scores(i,5),scores(i,3),scores(i,4),scores(i,2)));
  p=get(lh,'Position');
  p(1)=0.25;
  p(2)=0.15;
  set(lh,'Position',p);
  print(h,'-depsc2',fullfile(pbDir,sprintf('%d_pr.eps',iid)));
  print(h,'-djpeg95','-r36',fullfile(pbDir,sprintf('%d_pr_half.jpg',iid)));
  print(h,'-djpeg95','-r0',fullfile(pbDir,sprintf('%d_pr_full.jpg',iid)));
  %close(h);
end

function prplot(h,r,p,ti)
figure(h);
plot(r,p,'ko-');
box on;
grid on;
set(gca,'Fontsize',12);
set(gca,'XTick',[0 .25 .5 .75 1]);
set(gca,'YTick',[0 .25 .5 .75 1]);
set(gca,'XGrid','on');
set(gca,'YGrid','on');
xlabel('Recall');
ylabel('Precision');
title(ti);
axis square;
axis([0 1 0 1]);


function inls = countCorrespondences(M, Btree, epsilon)

    [idx, dist] = knnsearch(Btree, M');
    in_idx = find(dist<=epsilon);

    inls = length(idx(in_idx));

end


clear;close all;
im1=imread('page.png');
im2=imread('tshape.png');
bwim1=adaptivethreshold(im1,11,0.03,0);
bwim2=adaptivethreshold(im2,15,0.02,0);
subplot(2,2,1);
imshow(im1);
subplot(2,2,2);
imshow(bwim1);
subplot(2,2,3);
imshow(im2);
subplot(2,2,4);
imshow(bwim2);


function [M] = ROIcmap(nc,opt)
% [M] = ROIcmap([nc],[opt]);
% creates a colormap with
%           no color too close to grayscale,
%           no two consecutive colors too close
%           no colors exeedingly close to another in the map
%           no colors too close to a background color (optional)
%   nc: number of colors in map. Default is 64
%   opt: optional options structure
%      .show: Figure handle in which to show the map
%             Default is 1. Set to 0 for no show.
%             In show mode, you get to pick colors with mouse
%             and read in their values. Hit enter to exit from
%             this mode.
%      .state: State of the random number generator.
%              Default is 0.
%      .write: Name of file to write colormap into
%              Default is '', no writing. Use something like
%              ROI64s0.1D.cmap, for a 64 cols, seed 0 colormap.
%      .avoid: Color to avoid getting close to.
%      .verb: verbosioty. 1 is default. 0 is for quiet
% returns
%   M: The colormap.
%
%see also readXcol, rgbdectohex, and ScaleToMap
% Ziad S. Saad SSCC/NIMH/NIH, saadz@mail.nih.gov

if (nargin == 0),
   nc = 64;
   opt.show = 1;
elseif (nargin == 1),
   opt.show = 1;
end

if (isempty(nc)), nc = 64; end

if (~isfield(opt,'show') | isempty(opt.show)), opt.show = 1; end
if (~isfield(opt,'state') | isempty(opt.state)), opt.state = 0; end
if (~isfield(opt,'write') | isempty(opt.write)), opt.write = ''; end
if (~isfield(opt,'verb') | isempty(opt.verb)), opt.verb = 1; end
if (~isfield(opt,'avoid') | isempty(opt.avoid)), opt.avoid = []; end

%initialize rng
rand('state',opt.state);

M = zeros(nc,3);
alldiff_lim = 0.5; %between 0 and 1, controls how different all colors in map are.
               %The first few colors can be quite different, high alldiff_lim
               %The difference is adjusted as more colors are demanded.
g_lim = 0.2; %limit for too gray (0-1)
d_lim = 0.40; %limit for too dim (0-3)
b_lim = 2.2; %limit for too bright  (0-3)
for (i=1:1:nc),
   M(i,:) = rand(1,3);
   cnt = 0;
   %reject if too gray or too close to previous color
   while (  toogray(M(i,:), g_lim, d_lim, b_lim) |  tooclose(M,i, 0.6, alldiff_lim) | (~isempty(opt.avoid) & (sum(abs(M(i,:)-opt.avoid)) < 0.6))),
      M(i,:) = rand(1,3);
      cnt = cnt + 1;
      if (cnt > 2000), % too tight, relax
         alldiff_lim = max([0.95.*alldiff_lim 0.02]) ;
         d_lim = max([0.95.*d_lim 0.01]);
         b_lim = min([b_lim*1.05, 8.0]);
         if (opt.verb) fprintf(1,'Reduced alldiff_lim to %g, d_lim to %g, b_lim to %g\n', alldiff_lim, d_lim, b_lim); end
         cnt = 0;
      end
   end
   if (opt.verb) fprintf(1,'Color %d OK\n', i); end
end
if (opt.verb) fprintf(1,'alldiff_lim final was %g, d_lim final was %g, b_lim final was %g\n', alldiff_lim, d_lim, b_lim); end

if (~isempty(opt.write)),
   optw.OverWrite = 'p';
   wryte3(M, opt.write, optw);
end

if (opt.show),
   ShowCmap(M, opt.show);
end

return;

function [a] = toogray(c, g_lim, d_lim, b_lim)

   a = 0;
   dc = abs(c - mean(c));
   cs = sum(c);
   if (dc(1) < g_lim & dc(2) < g_lim & dc(3) < g_lim), a = 1; return; end
   if (cs < d_lim | cs > b_lim), a = 1; return; end
   return;

function [a] = tooclose(M,i,prev_lim, alldiff_lim)

   if (i==1), a = 0; return; end

   a = 1;

   %too close to previous ?
   dc = abs(M(i,:)-M(i-1,:));
   if (sum(dc) < prev_lim), return; end

   %too close to one before?
   if (i > 2),
      for (j=1:1:i-2),
         dc = abs(M(i,:)-M(j,:));
         if (dc(1) < alldiff_lim & dc(2) < alldiff_lim & dc(3) < alldiff_lim), return; end
      end
   end
   %OK if you get here
   a = 0;
   return;


% mycorr2 modified version of the 2D correlation
%         for the use with im2col and col2im
%         see GETPOINT
%
%
% $Id: mycorr2.m,v 2.0 2003/06/19 12:06:52 svoboda Exp $

% Note: It written in order to gain speed. The clarity of the code suffers accordingly

function R = mycorr2(X,G,Gn,Gn2)

% Gn  = G-mean(G);
% Gn2 = sqrt(sum(Gn.^2));

mX	= repmat(mean(X),size(X,1),1);
mXn = X - mX;
smX	= sum(mXn.^2);

numerator = (mXn'*Gn)';
denominator = smX*Gn2;

R = numerator./denominator;

return


function mtrFigureGASMatchedPathDist(machineDir, imageDir)

subjVec = {'tony_nov05','thor_nov05','sil_nov05'};
%subjVec = {'thor_nov05'};
threshVec = [1000];
ctFile = 'paths500k_5k.dat';
sttFile = 'stt_clip.dat';
distMatricesFile = 'distMatricesGAS.mat';
bSavePlot = 0;

for ss = 1:length(subjVec)
    subjDir = fullfile(machineDir,subjVec{ss});
    figure;

    fgDir = fullfile(subjDir, 'conTrack');
    disp(['cd ' fgDir]);
    cd(fgDir);
    [meanD, maxD] = mtrMatchPathways(fullfile(subjDir,'dt6.mat'), fullfile(subjDir,'fibers',sttFile), ctFile);

    % Save out the figures to the image dir
    subplot(2,1,1); hist((min(meanD,[],2)));
    xlabel('Mean distance to closest conTrack path (mm)');
    subplot(2,1,2); hist((min(maxD,[],2)));
    xlabel('Max distance to closest conTrack path (mm)');
    save(distMatricesFile,'meanD','maxD');

    if(bSavePlot)
        set(gcf,'Position',[504 687 436 259]);
        figFilename = fullfile(subjDir,imageDir,['distHist.png'])
        set(gcf,'PaperPositionMode','auto');
        print('-dpng', figFilename);
    end
end


%TRPRINT Compact display of homogeneous transformation
%
% TRPRINT(T, OPTIONS) displays the homogoneous transform in a compact
% single-line format.  If T is a homogeneous transform sequence then each
% element is printed on a separate line.
%
% S = TRPRINT(T, OPTIONS) as above but returns the string.
%
% TRPRINT T  is the command line form of above, and displays in RPY format.
%
% Options::
% 'rpy'        display with rotation in roll/pitch/yaw angles (default)
% 'euler'      display with rotation in ZYX Euler angles
% 'angvec'     display with rotation in angle/vector format
% 'radian'     display angle in radians (default is degrees)
% 'fmt', f     use format string f for all numbers, (default %g)
% 'label',l    display the text before the transform
%
% Examples::
%        >> trprint(T2)
%        t = (0,0,0), RPY = (-122.704,65.4084,-8.11266) deg
%
%        >> trprint(T1, 'label', 'A')
%               A:t = (0,0,0), RPY = (-0,0,-0) deg
%
% See also TR2EUL, TR2RPY, TR2ANGVEC.

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

function out = trprint(T, varargin)

    if ischar(T)
        % command form: trprint T
        trprint( evalin('base', T) );
        return;
    end

    opt.fmt = [];
    opt.mode = {'rpy', 'euler', 'angvec'};
    opt.radian = false;
    opt.label = '';

    opt = tb_optparse(opt, varargin);

    s = '';

    if size(T,3) == 1
        if isempty(opt.fmt)
            opt.fmt = '%g';
        end
        s = tr2s(T, opt);
    else
        if isempty(opt.fmt)
            opt.fmt = '%8.2g';
        end

        for i=1:size(T,3)
            % for each 4x4 transform in a possible 3D matrix
            s = char(s, tr2s(T(:,:,i), opt) );
        end
    end

    % if no output provided then display it
    if nargout == 0
        disp(s);
    else
        out = s;
    end
end

function s = tr2s(T, opt)
    % print the translational part if it exists
    if ~isempty(opt.label)
        s = sprintf('%8s: ', opt.label);
    else
        s = '';
    end
    if ~isrot(T)
        s = strcat(s, sprintf('t = (%s),', vec2s(opt.fmt, transl(T)')));
    end

    % print the angular part in various representations
    switch (opt.mode)
        case {'rpy', 'euler'}
            % angle as a 3-vector
            if strcmp(opt.mode, 'rpy')
                ang = tr2rpy(T);
                label = 'RPY';
            else
                ang = tr2eul(T);
                label = 'EUL';
            end
            if opt.radian
                s = strcat(s, ...
                    sprintf(' %s = (%s) rad', label, vec2s(opt.fmt, ang)) );
            else
                s = strcat(s, ...
                    sprintf(' %s = (%s) deg', label, vec2s(opt.fmt, ang*180.0/pi)) );
            end
        case 'angvec'
            % as a vector and angle
            [th,v] = tr2angvec(T);
            if isnan(v(1))
                s = strcat(s, sprintf(' R = nil') );
            elseif opt.radian
                s = strcat(s, sprintf(' R = (%sdeg | %s)', ...
                    sprintf(opt.fmt, th), vec2s(opt.fmt, v)) );
            else
                s = strcat(s, sprintf(' R = (%sdeg | %s)', ...
                    sprintf(opt.fmt, th*180.0/pi), vec2s(opt.fmt,v)) );
            end
    end
end

function s = vec2s(fmt, v)
    s = '';
    for i=1:length(v)
        s = strcat(s, sprintf(fmt, v(i)));
        if i ~= length(v)
            s = strcat(s, ', ');
        end
    end
end


function engine = belprop_mrf2_inf_engine(mrf2, varargin)
% BELPROP_MRF2_INF_ENGINE Belief propagation for MRFs with discrete pairwise potentials
% engine = belprop_mrf2_inf_engine(mrf2, ...)
%
% This is like belprop_inf_engine, except it is designed for mrf2, so is much faster.
%
% [ ... ] = belprop_mrf2_inf_engine(..., 'param1',val1, 'param2',val2, ...)
% allows you to specify optional parameters as name/value pairs.
% Parameters modifying behavior of enter_evidence are below [default value in brackets]
%
% max_iter - max. num. iterations [ 5*nnodes]
% momentum - weight assigned to old message in convex combination
%            (useful for damping oscillations) [0]
% tol      - tolerance used to assess convergence [1e-3]
% verbose - 1 means print error at every iteration [0]
%
% Parameters can be changed later using set_params

% The advantages of pairwise potentials are
% (1) we can compute messages using vector-matrix multiplication
% (2) we can easily specify the parameters: one potential per edge
% In contrast, potentials on larger cliques are more complicated to deal with.

nnodes = length(mrf2.adj_mat);

[engine.max_iter, engine.momentum, engine.tol, engine.verbose] = ...
    process_options(varargin, 'max_iter', [], 'momentum', 0, 'tol', 1e-3, ...
		   'verbose', 0);

if isempty(engine.max_iter) % no user supplied value, so compute default
  engine.max_iter = 5*nnodes;
  %if acyclic(mrf2.adj_mat, 0) --- can be very slow!
  %  engine.max_iter = nnodes;
  %else
  %  engine.max_iter = 5*nnodes;
  %end
end

engine.bel = cell(1, nnodes); % store results of enter_evidence here
engine.mrf2 = mrf2;

engine = class(engine, 'belprop_mrf2_inf_engine');


function [nconds cforecast_record cforecast_estimates]=panel3cf(N,n,m,p,k,q,cfconds,cfshocks,cfblocks,data_endo_a,data_exo_a,data_exo_p,It,Bu,Fperiods,const,beta_gibbs,D_record,gamma_record,CFt,Fband)

% initiate the cell recording the Gibbs sampler draws
cforecast_record={};
cforecast_estimates={};

% because conditional forecasts can be computed for many (potentially all) units, loop over units
for ii=1:N
% check wether there are any conditions on unit ii
temp=cfconds(:,:,ii);
nconds(ii,1)=numel(temp(cellfun(@(x) any(~isempty(x)),temp)));

   % if there are conditions
   if nconds(ii,1)~=0
   % prepare the elements for conditional forecast estimation, depending on the type of conditional forecasts
   temp1=cfconds(:,:,ii);
      if CFt==1
      temp2={};
      temp3=[];
      elseif CFt==2
      temp2=cfshocks(:,:,ii);
      temp3=cfblocks(:,:,ii);
      end
   % run the Gibbs sampler for unit ii
   cforecast_record(:,:,ii)=bear.cforecast(data_endo_a(:,:,ii),data_exo_a,data_exo_p,It,Bu,Fperiods,temp1,temp2,temp3,CFt,const,beta_gibbs(:,:,ii),D_record(:,:,ii),gamma_record(:,:,ii),n,m,p,k,q);
   % then obtain point estimates and credibility intervals
   cforecast_estimates(:,:,ii)=bear.festimates(cforecast_record(:,:,ii),n,Fperiods,Fband);

   % if there are no conditions, return empty elements
   elseif nconds(ii,1)==0
   cforecast_record(:,:,ii)=cell(n,1);
   cforecast_estimates(:,:,ii)=cell(n,1);
   end
end


function model = mpt_enumeration_mpmilp(Matrices,options)
% Variable bounds when all binary variables are relaxed
[global_lower,global_upper] = mpt_detect_and_improve_bounds(Matrices,Matrices.lb,Matrices.ub,Matrices.binary_var_index,options);

Matrices.lb = global_lower;
Matrices.ub = global_upper;
if any(Matrices.lb(end-Matrices.nx+1:end) == Matrices.ub(end-Matrices.nx+1:end))
    model = [];
    return
end

% Enumerate a sufficent set of binary cases
% (exploit SOS and pure binary constraints)
[enums,Matrices] = mpt_enumerate_binary(Matrices);

model = [];
for i = 1:size(enums,2);
    if options.verbose & rem(i,20)==0
        disp(['Binary node ' num2str(i) '/' num2str(size(enums,2))]);
    end
    % Create node problem
    lower = global_lower;
    upper = global_upper;
    lower(Matrices.binary_var_index) = enums(:,i);
    upper(Matrices.binary_var_index) = enums(:,i);
    % Pre-solve, solve and merge
    model = mpt_solvenode(Matrices,lower,upper,Matrices,model,options);
end


function [cl,clpos,clneg,clrpos,clrneg] = cluster_sigregions(cl,pthr,varargin)
% [cl,clpos,clneg,clrpos,clrneg] = cluster_ttest(clusters,pthr,[behavioral regressor])
% tor wager  Oct 9, 2003
%
% does t-tests on cluster timeseries and all voxels
% which should contain group data (e.g., con* data)
%
% if optional behavioral regressor is entered,
% computes cluster extent nonparametric p-values, and reports
% extent npm p-values for intercept and behavioral reg.
%
% writes output to cluster_ttest_output.txt

fid = fopen('cluster_ttest_output.txt','a');

if length(varargin) > 0, beh = varargin{1};,end

for i = 1:length(cl)

% df = n - k    primary threshold
tthr1 = abs(tinv(pthr,size(cl(i).all_data,1) - size(cl(i).ttest.t,1)));

% ----------------------------------------
% * do nonparametric permutation
% ----------------------------------------

if ~isfield(cl(i),'npm_ttest'),cl(i).npm_ttest = [];,end
if isfield(cl(i).npm_ttest,'tmax') & isfield(cl(i).npm_ttest,'textmax')
    tthr = cl(i).npm_ttest.tthr ;
    rthr = cl(i).npm_ttest.rthr ;
    tnegthr = cl(i).npm_ttest.tnegthr ;
    rnegthr = cl(i).npm_ttest.rnegthr ;
    tmax = cl(i).npm_ttest.tmax ;
    tmin = cl(i).npm_ttest.tmin ;
    rmax = cl(i).npm_ttest.rmax ;
    rmin = cl(i).npm_ttest.rmin ;
    textmax = cl(i).npm_ttest.textmax ;
    rextmax = cl(i).npm_ttest.rextmax ;
else

[tthr,rthr,tnegthr,rnegthr,tmax,rmax,tmin,rmin,textmax,rextmax] = ...
    npm_ttest(cl(i).all_data,1000,beh,pthr,cl(i).XYZ);
close

cl(i).npm_ttest.tthr = tthr;
cl(i).npm_ttest.rthr = rthr;
cl(i).npm_ttest.tnegthr = tnegthr;
cl(i).npm_ttest.rnegthr = rnegthr;
cl(i).npm_ttest.tmax = tmax;
cl(i).npm_ttest.rmax = rmax;
cl(i).npm_ttest.textmax = textmax;
cl(i).npm_ttest.rextmax = rextmax;
cl(i).npm_ttest.tmin = tmin;
cl(i).npm_ttest.rmin = rmin;

end

fprintf(fid,'\nCl.\tVoxels\tThreshold\tMask\tData\t\n');
fprintf(fid,'\tx\ty\tz\tVoxels\tCluster corr. p.\tMax. t\tCorrected p\tDirection of effect\tr\n');
fprintf(fid,'%3.0f\t%3.0f\t%3.4f\t%s\t%s\t\n',i,cl(i).numVox,pthr,cl(i).P,cl(i).imP(1,:));

% ----------------------------------------
% * get sig clusters
% ----------------------------------------

clpos(i).from_cluster = i;
clpos(i).M = cl(i).M;
clpos(i).voxSize = cl(i).voxSize;
clpos(i).threshold = tthr1;
clpos(i).title = 'Positive effects';

clrpos(i).from_cluster = i;
clrpos(i).M = cl(i).M;
clrpos(i).voxSize = cl(i).voxSize;
clrpos(i).threshold = tthr1;
clrpos(i).title = 'Positive correlations';

sig = cl(i).ttest.t > tthr1;

if any(sig(1,:))
    clpos(i).XYZ = cl(i).XYZ(:,find(sig(1,:)));
    clpos(i).XYZmm = cl(i).XYZmm(:,find(sig(1,:)));
    clpos(i).Z = cl(i).Z(:,find(sig(1,:)));
    clpos(i).t = cl(i).ttest.t(:,find(sig(1,:)));
    gopos(i) = 1;

    % print table entry
    wcl = spm_clusters(clpos(i).XYZ);
    clpos(i).clusters = wcl;
    for j = 1:max(wcl)

        wh = find(wcl==j);

        % corrected p

        xyz = mean(clpos(i).XYZmm(:,wh),2)';
        if length(xyz) == 1, xyz = clpos(i).XYZmm(:,wh)';,end
        nv = length(wh);

        if length(wh)>1,max_t = max(clpos(i).t(:,wh)'); max_t = max_t(1);,
        else, max_t = clpos(i).t(1,wh);
        end

        clpos(i).center = xyz;

        extcor_p = 1 - (sum(textmax <= nv) ./ length(textmax));
        max_p = 1 - (sum(tmax <= max_t) ./ length(tmax));

        fprintf(fid,'\t%3.0f\t%3.0f\t%3.0f\t%3.0f\t%3.4f\t%3.2f\t%3.4f\t+\n', ...
            xyz(1),xyz(2),xyz(3),nv,extcor_p,max_t,max_p)
    end

else
    gopos(i) = 0;
end

if any(sig(2,:))
    clrpos(i).XYZ = cl(i).XYZ(:,find(sig(2,:)));
    clrpos(i).XYZmm = cl(i).XYZmm(:,find(sig(2,:)));
    clrpos(i).Z = cl(i).Z(:,find(sig(2,:)));
    clrpos(i).t = cl(i).ttest.t(:,find(sig(2,:)));
    gorpos(i) = 1;

    % print table entry
    wcl = spm_clusters(clrpos(i).XYZ);
    clrpos(i).clusters = wcl;
    for j = 1:max(wcl)

        wh = find(wcl==j);

        % corrected p

        xyz = mean(clrpos(i).XYZmm(:,wh),2)';
        if length(xyz) == 1, xyz = clrpos(i).XYZmm(:,wh)';,end
        nv = length(wh);

        tmp = cl(i).all_data(:,find(sig(2,:)));
        tmp = tmp(:,wh);
        tmp = corrcoef([tmp beh]);
        tmp = tmp(end,1:end-1);
        max_r = max(tmp);
        if max_r > .99, warning('problem?'),keyboard,end

        if length(wh)>1,max_t = max(clrpos(i).t(:,wh)'); max_t = max_t(2);,
        else, max_t = clrpos(i).t(2,wh);
        end
        clrpos(i).center = xyz;

        extcor_p = 1 - sum(rextmax <= nv) ./ length(rextmax);
        max_p = 1 - sum(rmax <= max_t) ./ length(rmax);

        fprintf(fid,'\t%3.0f\t%3.0f\t%3.0f\t%3.0f\t%3.4f\t%3.2f\t%3.4f\t+\t%3.2f\n', ...
            xyz(1),xyz(2),xyz(3),nv,extcor_p,max_t,max_p,max_r);
    end

else
    gorpos(i) = 0;
end

% negative

clneg(i).from_cluster = i;
clneg(i).M = cl(i).M;
clneg(i).voxSize = cl(i).voxSize;
clneg(i).threshold = tthr1;
clneg(i).title = 'Negative effects';

clrneg(i).from_cluster = i;
clrneg(i).M = cl(i).M;
clrneg(i).voxSize = cl(i).voxSize;
clrneg(i).threshold = tthr1;
clrneg(i).title = 'Negative correlations';

sig = cl(i).ttest.t < -tthr1;

if any(sig(1,:))
    clneg(i).XYZ = cl(i).XYZ(:,find(sig(1,:)));
    clneg(i).XYZmm = cl(i).XYZmm(:,find(sig(1,:)));
    clneg(i).Z = cl(i).Z(:,find(sig(1,:)));
    clneg(i).t = cl(i).ttest.t(:,find(sig(1,:)));
    goneg(i) = 1;

    % print table entry
    wcl = spm_clusters(clneg(i).XYZ);
    clneg(i).clusters = wcl;
    for j = 1:max(wcl)

        wh = find(wcl==j);

        % corrected p

        xyz = mean(clneg(i).XYZmm(:,wh),2)';
        if length(xyz) == 1, xyz = clpos(i).XYZmm(:,wh)';,end
        nv = length(wh);

        if length(wh)>1,max_t = min(clneg(i).t(:,wh)'); max_t = max_t(1);,
        else, max_t = clneg(i).t(1,wh);
        end

        clneg(i).center = xyz;

        extcor_p = 1 - sum(textmax <= nv) ./ length(textmax);
        max_p = 1 - sum(tmin >= max_t) ./ length(tmin);

        fprintf(fid,'\t%3.0f\t%3.0f\t%3.0f\t%3.0f\t%3.4f\t%3.2f\t%3.4f\t-\n', ...
            xyz(1),xyz(2),xyz(3),nv,extcor_p,max_t,max_p);
    end

else
    goneg(i) = 0;
end

if any(sig(2,:))
    clrneg(i).XYZ = cl(i).XYZ(:,find(sig(2,:)));
    clrneg(i).XYZmm = cl(i).XYZmm(:,find(sig(2,:)));
    clrneg(i).Z = cl(i).Z(:,find(sig(2,:)));
    clrneg(i).t = cl(i).ttest.t(:,find(sig(2,:)));
    gorneg(i) = 1;

    % print table entry
    wcl = spm_clusters(clrneg(i).XYZ);
    clrneg(i).clusters = wcl;
    for j = 1:max(wcl)

        wh = find(wcl==j);

        % corrected p

        xyz = mean(clrneg(i).XYZmm(:,wh),2)';
        if length(xyz) == 1, xyz = clrneg(i).XYZmm(:,wh)';,end
        nv = length(wh);

        if length(wh)>1,max_t = min(clrneg(i).t(:,wh)'); max_t = max_t(2);,
        else, max_t = clrneg(i).t(2,wh);
        end

        tmp = cl(i).all_data(:,find(sig(2,:)));
        tmp = tmp(:,wh);
        tmp = corrcoef([tmp beh]);
        tmp = tmp(end,1:end-1);
        max_r = min(tmp);
        if max_r > .99, warning('problem?'),keyboard,end

        clrneg(i).center = xyz;

        extcor_p = 1 - sum(rextmax <= nv) ./ length(rextmax);
        max_p = 1 - sum(rmin >= max_t) ./ length(rmin);

        fprintf(fid,'\t%3.0f\t%3.0f\t%3.0f\t%3.0f\t%3.4f\t%3.2f\t%3.4f\t-\t%3.2f\t\n', ...
            xyz(1),xyz(2),xyz(3),nv,extcor_p,max_t,max_p,max_r);
    end

else
    gorneg(i) = 0;
end

end % loop through clusters

fprintf(fid,'\n')

% Make montage
try

if ~isempty(clpos) & ~isempty(clneg)
    montage_clusters([],cl(unique(cat(2,[clpos.from_cluster clneg.from_cluster]))),clpos,clneg,{'k' 'r' 'b'},'nooverlap')
    %hh=get(gcf,'Children');
    %for i = 1:length(hh),axes(hh(i));, h = findobj(gca,'FaceColor',[0 0 0]);, set(h,'FaceAlpha',.5) ,end
elseif ~isempty(clpos)
    montage_clusters([],cl(cat(2,clpos.from_cluster)),clpos,{'k' 'r'},'nooverlap')
elseif ~isempty(clneg)
    montage_clusters([],cl,clneg,{'k' 'b'},'nooverlap')
else
    disp('No main contrast effects')
end

if ~isempty(clrpos) & ~isempty(clrneg)
    montage_clusters([],cl(unique(cat(2,[clrpos.from_cluster clrneg.from_cluster]))),clrpos,clrneg,{'k' 'r' 'b'},'nooverlap')
elseif ~isempty(clrpos)
    montage_clusters([],cl(cat(2,clrpos.from_cluster)),clrpos,{'k' 'r'},'nooverlap')
elseif ~isempty(clrneg)
    montage_clusters([],cl(cat(2,clrneg.from_cluster)),clrneg,{'k' 'b'},'nooverlap')
else
    disp('No correlation effects')
end

catch
    disp('error with montage')
end

fclose(fid);

return


function scrollsubplot(dx,x,varargin);

% Cette fonction permet l'ajout d'un curseur dynamique sur les figures
% graphique tout en permettant l'utilisation de sous-graphiques subplot.
% Cette fonction est une variation de la fonction scrollplot de Wolfgang
% Stiegmaier et de scrollplotdemo de Steven Lord, slord@mathworks.com.
%
% Syntaxe
%   scrollsubplot(dx,x,h1,h2,...hn);
%
% Description
% dx: détermine la largeur de la fenetre
% x: vecteur temps
% h1...hn: les références aux sous-graphiques
%
% exemple
%
% temps = linspace(0,10*pi,100001);
% Y1 = sin(temps);
% Y2 = cos(temps);
% Y3 = sin(temps).^2;
%
% h1=subplot(3,1,1);
% plot(temps,Y1),grid;
% h2=subplot(3,1,2);
% plot(temps,Y2),grid;
% h3=subplot(3,1,3);
% plot(temps,Y3),grid;
% scrollsubplot(pi,temps,h1,h2,h3);
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% Benoit Cantin, 7 mars 2005. %%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Original message
% :Created by Steven Lord, slord@mathworks.com
% :Uploaded to MATLAB Central
% :http://www.mathworks.com/matlabcentral
% :7 May 2002
% :Permission is granted to adapt this code for your own use.
% :However, if it is reposted this message must be intact.

% get current axes
a=gca;
% This avoids flickering when updating the axis
set(gcf,'doublebuffer','on');
% Set appropriate axis limits and settings
set(a,'xlim',[0 dx]);
%set(a,'ylim',[min(y) max(y)]);

% Generate constants for use in uicontrol initialization
pos=get(a,'position');
xmax=max(x);
xmin=min(x);

% This will create a slider which is just underneath the axis
% but still leaves room for the axis labels above the slider
Newpos=[pos(1) pos(2)-0.1 pos(3) 0.05];

for i = 1:length(varargin);
    %initialize postion of plot
    set(varargin{i},'xlim',[xmin xmin+dx]);

    % Création des variables h1...hn
    eval(['h',num2str(i), ' = varargin{i};']);

    % Setting up callback string to modify XLim of axis (gca)
    % based on the position of the slider (gcbo)
    if i == 1;
        S=['set(h', num2str(i), ' ,''xlim'',get(gcbo,''value'')+[0 ' num2str(dx) '])'];
    else
        S=[S ', set(h', num2str(i), ' ,''xlim'',get(gcbo,''value'')+[0 ' num2str(dx) '])'];
    end
end

% Creating Uicontrol with initial value of the minimum of x
h=uicontrol('style','slider',...
    'units','normalized','position',Newpos,...
    'callback',S,'min',xmin,'max',xmax-dx,'value',xmin);


function [W,dist_H_V] = shepard(V,C,P,E,CE,p)
  % SHEPARD Compute shepard weights for a list of vertices, given a list of
  % samples and optionally a denominator power value.
  %
  % W = shepard(V,C)
  %
  % Inputs:
  %  V  list of vertex positions
  %  C  list of control vertices
  %  P  list of indices into C for point controls, { 1:size(C,1) }
  %  E  list of bones, pairs of indices into C, connecting control vertices,
  %    { [] }
  %  CE  list of "cage edges", pairs of indices into ***P***, connecting
  %    control ***points***. A "cage edge" just tells point boundary conditions
  %    to vary linearly along straight lines between the end points, and to be
  %    zero for all other handles. { [] }
  %  p  (optional) power for denominator, scalar or list same size as C {2}
  %
  % Outputs:
  %  W  weights, # vertices by # handles matrix of weights
  %
  % Example:
  %  % "Shape-aware" shepard weights
  %  D = permute(sum((repmat(V,[1,1,size(C,1)]) - ...
  %    permute(repmat(C,[1,1,n]),[3,2,1])).^2,2),[1,3,2]);
  %  [minD,b] = min(D);
  %  snap_C = V(b,:);
  %  [BV,ev,ed] = biharmonic_embedding(V,F,6);
  %  [W] = shepard(BV,BV(b,:));
  %
  %

  % check if either are 3D but really all z's are 0
  V_flat = size(V,2) == 3 && (sqrt(sum(V(:,3).^2)) < 1e-10);
  C_flat = size(C,2) == 3 && (sqrt(sum(C(:,3).^2)) < 1e-10);
  % is both are essentially 2D then ignore z-coords
  if((size(C,2) == 2 || C_flat) && (size(V,2) == 2 || V_flat))
    % ignore z coordinate
    V = V(:,1:2);
    C = C(:,1:2);
  end

  assert(size(C,2) == size(V,2));
  dim = size(C,2);

  % default p value
  if(~exist('p','var'))
    p = 2;
  end

  if(~exist('P','var'))
    P = 1:size(C,1);
  end

  if(~exist('E','var'))
    E = [];
  end

  if(~exist('CE','var'))
    CE = [];
  end

  assert(prod(size(P)) == max(size(P)));
  assert(isempty(E) || size(E,2) == 2);
  assert(isempty(CE) || size(CE,2) == 2);
  assert(isempty(E) || max(E(:))<=size(C,1));
  assert(isempty(CE) || max(CE(:))<=numel(P));
  assert(isempty(P) || max(P(:))<=size(C,1));
  assert(isempty(E) || min(E(:)) >= 1);
  assert(isempty(CE) || min(CE(:)) >= 1);
  assert(isempty(P) || min(P(:)) >= 1);

  % number of point controls
  np = numel(P);
  nb = size(E,1);
  % number of domain vertices
  n = size(V,1);

  % if p is a scalar convert it now to a list the same size as C
  if(prod(size(p)) == 1)
    p = repmat(p,np+nb,1);
  elseif ~isempty(CE)
    if 0 ~= std(p(CE(:)))
      error('Variable powers not supported with cage edges');
    end
  end

  dist_P_V = zeros(np,n);
  if(np > 0)
    % vectors from V to every P, where PmV(i,j,:) is the vector from domain
    % vertex j to handle i
    PmV = ...
      permute( ...
        permute(repmat(C(P,:),[1,1,n]),[3,2,1]) - ...
        repmat(V,[1,1,np]),[3,1,2]);
    % distance from V to every P, where dist_P_V(i,j) is the distance from domain
    % vertex j to point handle i
    dist_P_V = sqrt(sum(PmV.^2,3));
    % distance from each corner in P to the next corner so that edge_length(i)
  end

  dist_B_V = zeros(nb,n);
  if(nb > 0)
    % loop over bones
    for( ii = 1:nb )
      % project to line
      [t,sqr_d] = project_to_lines(V,C(E(ii,1),:),C(E(ii,2),:));
      d = sqrt(sqr_d);
      % if projected point falls outside of line segment take distance to
      % closest end point
      d(t<0) = sqrt(sum((V(t<0,:)-repmat(C(E(ii,1),:),size(V(t<0,:),1),1)).^2,2));
      d(t>1) = sqrt(sum((V(t>1,:)-repmat(C(E(ii,2),:),size(V(t>1,:),1),1)).^2,2));
      dist_B_V(ii,:) = d';
    end
  end

  dist_H_V = [dist_P_V ; dist_B_V];
  % power of each control point seen by each vertex in domain
  pp = repmat(p,1,n);
  W = 1.0./((dist_H_V).^pp);

  if(size(CE,1) > 0)
    % zero out previous entries in points with incident cage edges
    W(CE(:),:) = 0;
    % compute projection of each point to each line segment
    [T,sqrD] = project_to_lines(V,C(P(CE(:,1)),:),C(P(CE(:,2)),:));
    % compute weights for each cage edge as if it were a bone
    pce = (p(CE(:,1))+p(CE(:,2)))/2;
    [~,dist_CE_V] = shepard(V,C,[],P(CE),[],pce);
    % compute "bone weights" for each cage edge as they were before
    % normalization
    ppce = repmat(pce,1,n);
    WCE = 1.0./((dist_CE_V).^ppce)';
    % clamp distances
    T(T<0) = 0;
    T(T>1) = 1;
    % multiply "bone weight" by projection parameter and sum up
    II = repmat(1:n,1,2*size(CE,1))';
    JJ = reshape(repmat(CE(:)',n,1),[],1);
    VV = [WCE(:).*(1-T(:)); WCE(:).*T(:)];
    W = W+sparse(II,JJ,VV,n,np+nb)';
    % cage edges on-samples
    on_sample = dist_CE_V < eps;
    [I,J] = find(on_sample);
    W(:,any(on_sample)) = 0;
    % grab T transpose
    TT = T';
    W(sub2ind(size(W),CE(I,1),J)) = 1-TT(on_sample);
    W(sub2ind(size(W),CE(I,2),J)) = TT(on_sample);
  end

  % Handle degenrate case that a control point is on a mesh vertex
  % snap vertices close to corners
  on_sample = dist_H_V < eps;
  W(:,any(on_sample,1)) = 0;
  W(on_sample) = 1;

  % normalize W
  W = W./repmat(sum(W,1),np+nb,1);

  % we've made W transpose
  W = W';

end


function M = setdiag(M, v)
% SETDIAG Set the diagonal of a matrix to a specified scalar/vector.
% M = set_diag(M, v)

n = length(M);
if length(v)==1
  v = repmat(v, 1, n);
end
J = 1:n+1:n^2;
M(J) = v;

%M = triu(M,1) + tril(M,-1) + diag(v);


function outstring =  demorse(wavfile);

%demorse a morsed input wav file

vis_on = 0;

threshold = 0.05;

x = wavread(wavfile);

% half-wave rectify x
x2 = abs(x);

% slow-wave filter
y = filter(ones(1,20)/20,1, x2);

% threshold (digitize) y
z = y > threshold;
% z is now effectively our morse signal

if vis_on
   figure(1);
   subplot(3,1,1);
   plot(x, 'r');
   title('original signal');

   subplot(3,1,2);
   plot(y);
   title('HWR + Slow-wave filter -> envelope');
   subplot(3,1,3);
   plot(z, 'o', 'MarkerSize', 2);
   title('Digitized Morse signal')

end

%zero pad z so we always start with an onset
z = [zeros(10,1); z];

% id tones/spaces   -----------------------
% --> find changes between 0/1 and 1/0

b = diff(z);
% figure(3); plot(b, '.');
% 1: change from 1 to 0
% 0: no change
% -1: change from 0 to 1

c = b(b~=0);
c2 = find(b~=0);

tokens = -c .* diff([0; c2]);
% value == length of token
% sign == tone/space

% id shorts/longs  -----------------------

% since short/long should be bi-modal dist, a regular average should give
% us a good cutoff point to distinguish between the two? (assuming equal
% counts of short and long...)
% use mean as simple cutoff point; smarter algorithms can get smarter about
% this classification if they want to.

% 1: short, 2: long, +: tone, -: space
tokens2 = tokens;

% cutoff tones, cutoff spaces;
cut_t = mean(tokens2(tokens2>0));
cut_s = mean(tokens2(tokens2<0));

tokens2(tokens > 0 & tokens < cut_t) = 1;
tokens2(tokens > 0 & tokens > cut_t) = 2;
tokens2(tokens < 0 & tokens > cut_s) = -1;
tokens2(tokens < 0 & tokens < cut_s) = -2;

% now tokens 2 is a string of -1s, -2s, 1s, 2s, can trim first known space;
% put final endstop at end
tokens2 = [tokens2(2:end); -2];

% can drop little spaces, b/c they don't matter when parsing;
tokens2(tokens2 == -1) = [];
tokens3 = tokens2;
tokens4 = {};
ctr = 1;
start_idx = 1;

%parse
toparse = find(tokens3(start_idx:end) == -2);

for j=1:length(toparse)
   a = toparse(j);
   temp = tokens3(start_idx:a-1);
   tokens4{j} = temp;
   % zeropad for easy comparison
   %tokens4{j} = [tokens4{j}; zeros(length(tokens4{j}), 1)];
   start_idx = a+1;

end

% now tokens4 is de-codeable tokens... proceed to setup lookups
% letters
code{1} = [1 2 ];
code{2} = [2 1 1 1];
code{3} = [2 1 2 1];
code{4} = [2 1 1];
code{5} = [1];
code{6} = [1 1 2 1];
code{7} = [2 2 1];
code{8} = [1 1 1 1];
code{9} = [1 1];
code{10} = [1 2 2 2];
code{11} = [2 1 2];
code{12} = [1 2 1 1];
code{13} = [2 2];
code{14} = [2 1];
code{15} = [2 2 2];
code{16} = [1 2 2 1];
code{17} = [1 2 1 2];
code{18} = [1 2 1];
code{19} = [1 1 1];
code{20} = [2];
code{21} = [1 1 2];
code{22} = [1 1 1 2];
code{23} = [1 2 2];
code{24} = [2 1 1 2];
code{25} = [2 1 2 2];
code{26} = [2 2 1 1];

% punct
code{27} = [1 2 1 2 1 2];
code{28} = [2 2 1 1 2 2];
code{29} = [1 1 2 2 1 1];
code{30} = [2 1 1 2 1];

% numbers

code{31} = [1 2 2 2 2];
code{32} = [1 1 2 2 2];
code{33} = [1 1 1 2 2];
code{34} = [1 1 1 1 2];
code{35} = [1 1 1 1 1];
code{36} = [2 1 1 1 1];
code{37} = [2 2 1 1 1];
code{38} = [2 2 2 1 1];
code{39} = [2 2 2 2 1];
code{40} = [2 2 2 2 2];

decode{1} = 'A';
decode{2} = 'B';
decode{3} = 'C';
decode{4} = 'D';
decode{5} = 'E';
decode{6} = 'F';
decode{7} = 'G';
decode{8} = 'H';
decode{9} = 'I';
decode{10} = 'J';
decode{11} = 'K';
decode{12} = 'L';
decode{13} = 'M';
decode{14} = 'N';
decode{15} = 'O';
decode{16} = 'P';
decode{17} = 'Q';
decode{18} = 'R';
decode{19} = 'S';
decode{20} = 'T';
decode{21} = 'U';
decode{22} = 'V';
decode{23} = 'W';
decode{24} = 'X';
decode{25} = 'Y';
decode{26} = 'Z';
decode{27} = '.';
decode{28} = ',';
decode{29} = '?';
decode{30} = '/';
decode{31} = '1';
decode{32} = '2';
decode{33} = '3';
decode{34} = '4';
decode{35} = '5';
decode{36} = '6';
decode{37} = '7';
decode{38} = '8';
decode{39} = '9';
decode{40} = '0';

% compare tokens to tables

out1 = [];

for j = 1:length(tokens4)
    %zero pad temp_tok
    temp_tok = [tokens4{j}; zeros(6 - length(tokens4{j}), 1)];
    for k = 1:length(code)
        if (temp_tok == [code{k}'; zeros(6 - length(code{k}), 1)]);
            out1(j) = char(decode{k});
            %display(decode{k})
        end

    end

    % if didn't find a match
    if isempty(out1(j))
        out1(j) = '_';
    end

end

% semi-prettify
outstring = 32*ones(2*length(out1),1);
outstring(2:2:end) = out1;
outstring = char(outstring');

%display('demorsed message:')
%display(outstring);


%% Example
% real time control of the KUKA iiwa 7 R 800
% Moving first joint of the robot, using a sinisoidal function

% An example script, it is used to show how to use the different
% functions of the KUKA Sunrise matlab toolbox

% First start the server on the KUKA iiwa controller
% Then run this script using Matlab

% Copyright: Mohammad SAFEEA, 15th of June 2017

% Important: Be careful when runnning the script, be sure that no human, nor obstacles
% are around the robot

close all,clear all;clc;

warning('off')

ip='172.31.1.147'; % The IP of the controller
% start a connection with the server
global t_Kuka;
t_Kuka=net_establishConnection( ip );

if ~exist('t_Kuka','var') || isempty(t_Kuka) || strcmp(t_Kuka.Status,'closed')
  disp('Connection could not be establised, script aborted');
  return;
end

%% Go to initial position

jPos={0,0,0,0,0,0,0};

setBlueOn(t_Kuka); % turn on blue light

relVel=0.5;
movePTPJointSpace( t_Kuka , jPos, relVel); % move to initial configuration

%% Start direct servo in joint space
realTime_startDirectServoJoints(t_Kuka);
%% Some variables
w=2.0; % motion constants, frequency rad/sec
A=pi/6; % motion constants, amplitude of motion
dt=0;
counter=0;
%% Initiate timing
tic;
%% Control loop
try
    daCount=0;
    while(dt<(80*pi/w))
        if daCount==0
            t0=toc; % calculate initial time
            t_0=toc;
            daCount=1;
        end
     %% perform trajectory calculation here
      time=toc;
      dt=time-t0;
      jPos{1}=A*(1-cos(w*dt));
      %% Send joint positions to robot
        if(toc-t_0>0.003)
            counter=counter+1;
            sendJointsPositionsf( t_Kuka ,jPos);
            t_0=toc;
        end
    end
    tstart=t0;
    tend=time;
    rate=counter/(tend-tstart);
    %% Stop the direct servo motion
    realTime_stopDirectServoJoints( t_Kuka );
    fprintf('\nTotal execution time is %f: \n',tend-t0 );
    fprintf('\nThe rate of joint nagles update per second is: \n');
    disp(rate);
    fprintf('\n')
    pause(2);
    %% turn off light
    setBlueOff(t_Kuka);
    net_turnOffServer( t_Kuka )
    disp('Direct servo motion completed successfully')
    warning('on')
    return;
catch
    % in case of error turn off the server
    net_turnOffServer( t_Kuka );
    disp('Error during execution the direct servo motion')
    fclose(t_Kuka);
    warning('on')
end


classdef prtClassMilVbDpGmm < prtClass
    % prtClassMilVbDpGmm VBDPGMM MIL Classifier
    %
    %  A variational bayes classifier for multiple instance data.  There
    %  are a number of parameters available for pruning/training.
    %  Performnace should (in theory) be somewhat invariant to these.  See
    %  the text of Manandhar et al., for descriptions.
    %
    %  You may need to reduce the dimensionality of the data being used via
    %  PCA, PLS, or some other pre-processing prior to using
    %  prtClassMilVbDpGmm.  We also recommend making sure the data is
    %  approximately zero-mean, unit-variance in each of the columns.
    %
    %  dsTrain = prtDataGenMultipleInstance;
    %  dsTest = prtDataGenMultipleInstance;
    %  class = prtClassMilVbDpGmm;
    %  class = class.train(dsTrain);
    %  yOutTrain = class.run(dsTrain);
    %  yOutTest = class.run(dsTest);
    %
    %  [pfTrain,pdTrain] = prtScoreRoc(yOutTrain);
    %  [pfTest,pdTest] = prtScoreRoc(yOutTest);
    %  subplot(1,1,1);
    %  plot(pfTrain,pdTrain,pfTest,pdTest);
    %

    properties (SetAccess=private)
        name = 'MilVbDpGmm'
        nameAbbreviation = 'MilVbDpGmm'
        isNativeMary = false;  % False
    end

    properties
        maxVbIter = 300;

        K0 = 5;
        K1 = 5;
        gamma_0_1_m0 = 1;
        gamma_0_2_m0 = .001;
        gamma_0_1_m1 = 1;
        gamma_0_2_m1 = .001;
        alpha = [10 10];

        rvH1
        rvH0
    end

    properties (Hidden)

        initialFit = [];
        %Dependent on "d"
        v_0_m
        v_1_m
        beta_1_m
        beta_0_m

        rho_0_1
        rho_0_0
        Phi_0_1
        Phi_0_0
    end

	methods
		function self = prtClassMilVbDpGmm(varargin)

			self = prtUtilAssignStringValuePairs(self,varargin{:});

            self.classTrain = 'prtDataSetClassMultipleInstance';
            self.classRun = 'prtDataSetClassMultipleInstance';
            self.classRunRetained = false;
        end

    end

    methods (Access=protected, Hidden = true)
		function self = trainAction(self,dsMil)

            milStruct = dsMil.data;
            x = cat(1,milStruct.data);

            bagLabels = dsMil.targets;
            expandedBagLabels = dsMil.expandedTargets;

            bagIndices = dsMil.getBagInds;
            uniqueBagIndices = unique(bagIndices);

            d = size(x,2);

            self.v_0_m = d+2;
            self.v_1_m = d+2;
            self.beta_1_m = d;
            self.beta_0_m = d;
            self.rho_0_0 = zeros(1,d);
            self.rho_0_1 = zeros(1,d);

            self.Phi_0_0 = eye(d)*d;
            self.Phi_0_1 = eye(d)*d;

%             params = struct('K0',5,'K1',5,'gamma_0_1_m0',1,'gamma_0_2_m0',.001,'gamma_0_1_m1',1,'gamma_0_2_m1',.001,...
%                 'v_0_m',d+2,'v_1_m',d+2,'beta_1_m',d,'beta_0_m',d, ...
%                 'rho_0_1',zeros(1,d),'rho_0_0',zeros(1,d),'Phi_0_1',eye(d)*d,'Phi_0_0',eye(d)*d,'alpha',[4 1]*10000);

            [rvH0,rvH1,xH0,xH1] = initialize(self,{milStruct.data},bagLabels);
            %self.initialFit = self.calculateDataLogLikelihood(self,xH0,xH1);
            ll0 = sum(rvH0.logPdf(xH0));
            ll1 = rvH0.pdf(xH1).*self.alpha(1)/sum(self.alpha) + rvH1.pdf(xH1).*self.alpha(2)/sum(self.alpha);
            self.initialFit = ll0 + sum(log(ll1));

            [p0,pp] = rvH0.pdf(x);
            phi_0_i = bsxfun(@rdivide,pp,sum(pp,2));

            [p1,pp] = rvH1.pdf(x);
            phi_1_i = bsxfun(@rdivide,pp,sum(pp,2));
            phi_1_i(expandedBagLabels == 0,:) = 0;
            p1(expandedBagLabels ==0) = 0;

            phi_1_M = p1./(p0 + p1);
            phi_0_M = 1-phi_1_M;

            locEps = 1e-6;

            for vbIter = 1:self.maxVbIter
                %Equations 67 and 68; note, these are different from 20-21 (!)
                %     gamma_i_1_m0 = sum(bsxfun(@times,phi_0_M,phi_0_i)) + self.gamma_0_1_m0;
                %     gamma_i_2_m0 = cat(2,0,cumsum(gamma_i_1_m0(1:end-1),2)) + self.gamma_0_2_m0;
                %
                %     gamma_i_1_m1 = sum(bsxfun(@times,phi_1_M,phi_1_i)) + self.gamma_0_1_m1;
                %     gamma_i_2_m1 = cat(2,0,cumsum(gamma_i_1_m1(1:end-1),2)) + self.gamma_0_2_m1;

                %Equations 20-21
                gamma_i_1_m0 = sum(bsxfun(@times,phi_0_M,phi_0_i)) + self.gamma_0_1_m0;
                gamma_i_2_m0 = fliplr(cumsum(fliplr(gamma_i_1_m0))) - gamma_i_1_m0 + self.gamma_0_2_m0;

                gamma_i_1_m1 = sum(bsxfun(@times,phi_1_M,phi_1_i)) + self.gamma_0_1_m1;
                gamma_i_2_m1 = fliplr(cumsum(fliplr(gamma_i_1_m1))) - gamma_i_1_m1 + self.gamma_0_2_m1;

                %Update priors... H0 Gaussian; Equations 77-79 (H0)
                respMat = bsxfun(@times,phi_0_M,phi_0_i);
                nBar0 = sum(respMat)+locEps;  %avoid 1./0 problems
                for cluster = 1:self.K0
                    mu_i_0(cluster,:) = 1/nBar0(cluster)*sum(bsxfun(@times,respMat(:,cluster),x));

                    xx = bsxfun(@times,sqrt(respMat(:,cluster)),bsxfun(@minus,x,mu_i_0(cluster,:)));
                    c = 1/nBar0(cluster)*xx'*xx;
                    cov_i_0(cluster,:) = c(:);
                end

                %Update priors... H1 Gaussian; Equations 77-79
                respMat = bsxfun(@times,phi_1_M,phi_1_i);
                nBar1 = sum(respMat)+locEps;
                for cluster = 1:self.K1
                    mu_i_1(cluster,:) = 1/nBar1(cluster)*sum(bsxfun(@times,respMat(:,cluster),x));

                    xx = bsxfun(@times,sqrt(respMat(:,cluster)),bsxfun(@minus,x,mu_i_1(cluster,:)));
                    c = 1/nBar1(cluster)*xx'*xx;
                    cov_i_1(cluster,:) = c(:);
                end

                %Updating hyper-parameters; eqns 81-84
                nu_0 = self.v_0_m + nBar0;
                nu_1 = self.v_1_m + nBar1;

                beta_0 = self.beta_0_m + nBar0;
                beta_1 = self.beta_1_m + nBar1;

                for cluster = 1:self.K0
                    rho_0(cluster,:) = self.beta_0_m*self.rho_0_0 + nBar0(cluster)*mu_i_0(cluster,:);
                    rho_0(cluster,:) = rho_0(cluster,:)./beta_0(cluster);

                    %See equation 84; Note: this is different from Equation 84 - 84 is
                    %wrong
                    %covPart = nBar0(cluster) * reshape(cov_i_0(cluster,:),d,d) * ((mu_i_0(cluster,:) - rho_0(cluster,:))'*(mu_i_0(cluster,:) - rho_0(cluster,:)));
                    covPart = nBar0(cluster) * self.beta_0_m * ((mu_i_0(cluster,:) - self.rho_0_0)'*(mu_i_0(cluster,:) - self.rho_0_0));
                    covPart = covPart ./ beta_0(cluster);

                    phiCov = self.Phi_0_0 + nBar0(cluster)*reshape(cov_i_0(cluster,:),d,d) + covPart;
                    Phi_0(cluster,:) = phiCov(:)';
                end

                for cluster = 1:self.K1
                    rho_1(cluster,:) = self.beta_1_m*self.rho_0_1 + nBar1(cluster)*mu_i_1(cluster,:);
                    rho_1(cluster,:) = rho_1(cluster,:)./beta_1(cluster);

                    %See equation 84; Note: this is different from Equation 84 - 84 is
                    %wrong
                    %covPart = nBar1(cluster) * reshape(cov_i_1(cluster,:),d,d) * ((mu_i_1(cluster,:) - rho_1(cluster,:))'*(mu_i_1(cluster,:) - rho_1(cluster,:)));
                    %         covPart = nBar1(cluster) * reshape(cov_i_1(cluster,:),d,d) * ((mu_i_1(cluster,:) - self.rho_0_0)'*(mu_i_1(cluster,:) - self.rho_0_0));
                    covPart = nBar1(cluster) * self.beta_1_m * ((mu_i_1(cluster,:) - self.rho_0_1)'*(mu_i_1(cluster,:) - self.rho_0_1));
                    covPart = covPart ./ beta_1(cluster);

                    phiCov = self.Phi_0_1 + nBar1(cluster)*reshape(cov_i_1(cluster,:),d,d) + covPart;
                    Phi_1(cluster,:) = phiCov(:)';
                end

                % <log_pi>  Equation 102
                matGamma = psi(gamma_i_1_m0) - psi(gamma_i_1_m0 + gamma_i_2_m0);
                matGamma2 = psi(gamma_i_2_m0) - psi(gamma_i_1_m0 + gamma_i_2_m0);
                log_pi_0 = matGamma + cat(2,0,cumsum(matGamma2(1:end-1)));

                matGamma = psi(gamma_i_1_m1) - psi(gamma_i_1_m1 + gamma_i_2_m1);
                matGamma2 = psi(gamma_i_2_m1) - psi(gamma_i_1_m1 + gamma_i_2_m1);
                log_pi_1 = matGamma + cat(2,0,cumsum(matGamma2(1:end-1)));

                % <Log \eta_m>  Equation 105
                nBarM0 = self.alpha(1) +  sum(phi_0_M(expandedBagLabels == 1,:));
                nBarM1 = self.alpha(2) +  sum(phi_1_M(expandedBagLabels == 1,:));
                log_eta = psi([nBarM0,nBarM1]) - psi(nBarM1+nBarM0);

                % <log det ||>
                for cluster = 1:self.K0
                    temp_det0 = sum(psi((nu_0(cluster) + 1 - (1:d))/2));
                    temp_det0 = temp_det0 + d*log(2);
                    log_det_0(cluster) = temp_det0 + log(det(reshape(Phi_0(cluster,:),d,d)^-1));
                end
                for cluster = 1:self.K1
                    temp_det1 = sum(psi((nu_1(cluster) + 1 - (1:d))/2));
                    temp_det1 = temp_det1 + d*log(2);
                    log_det_1(cluster) = temp_det1 + log(det(reshape(Phi_1(cluster,:),d,d)^-1));
                end

                %Equatioin 113
                for cluster = 1:self.K0
                    c = reshape(Phi_0(cluster,:),d,d);
                    %         v = prtUtilCalcDiagXcInvXT(bsxfun(@minus,x,mu_i_0(cluster,:)),c);
                    v = prtUtilCalcDiagXcInvXT(bsxfun(@minus,x,rho_0(cluster,:)),c);
                    inner_0_n(:,cluster) = d/beta_0(cluster) + nu_0(cluster).*v;
                end

                for cluster = 1:self.K1
                    c = reshape(Phi_1(cluster,:),d,d);
                    %         v = prtUtilCalcDiagXcInvXT(bsxfun(@minus,x,mu_i_1(cluster,:)),c);
                    v = prtUtilCalcDiagXcInvXT(bsxfun(@minus,x,rho_1(cluster,:)),c);
                    inner_1_n(:,cluster) = d/beta_1(cluster) + nu_1(cluster).*v;
                end

                for cluster = 1:self.K0
                    eLogPdf0(:,cluster) = -d*log(2*pi) + log_det_0(cluster) - inner_0_n(:,cluster);
                end
                eLogPdf0 = eLogPdf0*1/2;

                for cluster = 1:self.K1
                    eLogPdf1(:,cluster) = -d*log(2*pi) + log_det_1(cluster) - inner_1_n(:,cluster);
                end
                eLogPdf1 = eLogPdf1*1/2;

                phiHat0 = bsxfun(@plus,eLogPdf0,log_pi_0);
                phiHat1 = bsxfun(@plus,eLogPdf1,log_pi_1);
                phiHat1(expandedBagLabels == 0,:) = -inf;

                phi_1_i = bsxfun(@rdivide,exp(phiHat1),sum(exp(phiHat1),2));
                phi_0_i = bsxfun(@rdivide,exp(phiHat0),sum(exp(phiHat0),2));

                phi_1_i(exp(phiHat1) == 0) = 0;
                phi_0_i(exp(phiHat0) == 0) = 0;

                %
                n1 = (log(sum(exp(phiHat1),2))+log_eta(2));
                n0 = (log(sum(exp(phiHat0),2))+log_eta(1));
                phi_1_M = exp(n1)./(exp(n1)+exp(n0));
                phi_1_M(expandedBagLabels == 0) = 0;

                phi_0_M = 1-phi_1_M;

                for i = 1:size(rho_0,1)
                    c = Phi_0(i,:)./nu_0(i);
                    c = reshape(c,d,d);
                    if vbIter == 1
                        mm0(i) = prtRvMvn('mu',rho_0(i,:),'sigma',c);
                    else
                        mm0(i).mu = rho_0(i,:);
                        mm0(i).sigma = c;
                    end
                end
                for i = 1:size(rho_1,1)
                    c = Phi_1(i,:)./nu_1(i);
                    c = reshape(c,d,d);
                    if vbIter == 1
                        mm1(i) = prtRvMvn('mu',rho_1(i,:),'sigma',c);
                    else
                        mm1(i).mu = rho_1(i,:);
                        mm1(i).sigma = c;
                    end
                end

                pi0 = exp(log_pi_0); pi0 = pi0./sum(pi0);
                rv0 = prtRvGmm('nComponents',size(mu_i_0,1),'mixingProportions',pi0,'components',mm0);

                pi1 = exp(log_pi_1); pi1 = pi1./sum(pi1);
                rv1 = prtRvGmm('nComponents',size(mu_i_1,1),'mixingProportions',pi1,'components',mm1);

                self.rvH1 = rv1;
                self.rvH0 = rv0;

                drawnow;
                if ~mod(vbIter,100);
                    disp(vbIter)
                    subplot(2,4,1);
                    stem(exp(log_pi_0));
                    subplot(2,4,2);
                    stem(exp(log_eta))
                    subplot(2,4,3);
                    stem(exp(log_pi_1));
                    fprintf('exp(log_pi_0): %.2f\n',exp(log_pi_0))
                    fprintf('exp(log_pi_1): %.2f\n',exp(log_pi_1))

                    subplot(2,4,4);
                    self.isTrained = true;
                    yOut = self.run(dsMil);
                    prtScoreRoc(yOut);

                    subplot(2,4,5:8);
                    if d > 1
                        plot(x(expandedBagLabels == 0,1),x(expandedBagLabels == 0,2),'b.');
                        hold on;
                        plot(x(expandedBagLabels == 1,1),x(expandedBagLabels == 1,2),'r.');

                        for i = 1:size(rho_0,1)
                            c = Phi_0(i,:)./nu_0(i);
                            c = reshape(c,d,d);
                            if d > 1
                                %                                 plotMvnEllipse(rho_0(i,1:2),c(1:2,1:2),1);
                            end
                            mm0(i) = prtRvMvn('mu',rho_0(i,:),'sigma',c);
                        end
                        for i = 1:size(rho_1,1)
                            c = Phi_1(i,:)./nu_1(i);
                            c = reshape(c,d,d);
                            if d > 1
                                %                                 plotMvnEllipse(rho_1(i,1:2),c(1:2,1:2),1);
                            end
                            mm1(i) = prtRvMvn('mu',rho_1(i,:),'sigma',c);
                        end
                        hold on; h = plot(rho_0(:,1),rho_0(:,2),'ko'); set(h,'MarkerFaceColor','k');
                        hold on; h = plot(rho_1(:,1),rho_1(:,2),'go'); set(h,'MarkerFaceColor','g');

                        hold off;
                    end
                    drawnow;
                end

            end
        end

        function yOut = runAction(self,dsMil)

            for n = 1:dsMil.nObservations
                milStruct = dsMil.data(n);
                data = milStruct.data;

                h1 = self.rvH1.logPdf(data);
                h0 = self.rvH0.logPdf(data);
                l0 = h0'-prtUtilSumExp([h0';h1']);

                h0LogProbabilityBag = sum(l0);
                %exp(h0LogProbabilityBag)

                y(n,1) = 1-exp(h0LogProbabilityBag);
            end

            yOut = prtDataSetClass(y,dsMil.targets);
        end

        function [h0Mix,h1Mix,xH0,xH1] = initialize(self,xBag,bagLabels)

            %H0 Kmeans
            xH0 = cat(1,xBag{bagLabels == 0});
            idx0 = kmeans(xH0,self.K0,'Replicates',1,'EmptyAction','singleton','MaxIter',100);
            uIds = unique(idx0);

            for idx = 1:length(uIds)
                rvH0struct(idx).mean = mean(xH0(idx0 == idx,:),1);

                try
                    rvH0struct(idx).cov = cov(xH0(idx0 == idx,:));
                    chol(rvH0struct(idx).cov);
                    assert(size(xH0,2) == size(rvH0struct(idx).cov,2));
                catch ME
                    rvH0struct(idx).cov = diag(var(xH0(idx0 == idx,:))) + eye(size(xH0,2));
                end

                rvH0(idx) = prtRvMvn('mu',rvH0struct(idx).mean,'sigma',rvH0struct(idx).cov);
                pi(idx) = sum(idx0 == idx)./length(idx0);

            end
            pi = pi./sum(pi);
            h0Mix = prtRvGmm('nComponents',length(rvH0),'mixingProportions',pi,'components',rvH0);

            %Least likely H1
            h1Bags = xBag(bagLabels == 1);
            for h1Ind = 1:length(h1Bags)
                ll = h0Mix.logPdf(h1Bags{h1Ind});
                [~,ind] = min(ll);
                xH1(h1Ind,:) = h1Bags{h1Ind}(ind,:);
            end

            %H1 Kmeans
            idx1 = kmeans(xH1,self.K1,'Replicates',1,'EmptyAction','singleton','MaxIter',100);
            uIds = unique(idx1);

            pi = [];
            for idx = 1:length(uIds)
                rvH1struct(idx).mean = mean(xH1(idx1 == idx,:),1);

                try
                    rvH1struct(idx).cov = cov(xH1(idx0 == idx,:));
                    chol(rvH1struct(idx).cov);
                catch ME
                    rvH1struct(idx).cov = diag(var(xH1)) + eye(size(xH1,2));
                end

                rvH1(idx) = prtRvMvn('mu',rvH1struct(idx).mean,'sigma',rvH1struct(idx).cov);
                pi(idx) = sum(idx1 == idx)./length(idx1);

            end
            pi = pi./sum(pi);
            h1Mix = prtRvGmm('nComponents',length(rvH1),'mixingProportions',pi,'components',rvH1);
        end
    end
end


function x = rstepHess(obj,x0,dx,grad0)
alpha = sqrt(eps);
x = x0 + alpha*dx;
[y,deriv] = obj(x);
g = deriv();
x = (g-grad0)/alpha;


function [ y,dzdy ] = pad_data_1d(  I,P,dzdy)
%PAD_DATA Summary of this function goes here
%   Detailed explanation goes here

    if(length(P)==1)
        P=ones(1,2)*P;
    end

    if isempty(dzdy)
        %%forward
        original_size=size(I);
        new_size=original_size;
        new_size(1)=new_size(1)+P(1)+P(2);
        y=zeros(new_size,'like',I);
        y(1+P(1):P(1)+original_size(1),:,:)=I;

    else
        %backward
        new_size=size(dzdy);
        original_size=new_size;
        original_size(1)=original_size(1)-P(1)-P(2);
        y=I(1+P(1):P(1)+original_size(1),:,:);
        dzdy=dzdy(1+P(1):P(1)+original_size(1),:,:);

    end

end


function [cl,cd] = bpolar(BladeState,AC)
% To make your own bpolar:
% BladeState is the state of the blade element with matrix components:
% BladeState =
%            M: Mach number
%        alpha: angle of attack (radians)
%            r: radius location (nondimensional)
%          psi: azimuth location (radians)
%     alphadot: rate of change of angle of attack (rad/s)
%           Re: Reynolds number
% all components on BladeState inputs are m x n, where n is the number of
% blade elements, and m is the uniformly distributed number of azimuth
% elements.
%
% NB: this function should run VERY fast, so avoid loops and extensive
% interpolation.
% see also lininterp1f
%
% AC is a structure with AC data (see documentation)
%
% cl and cd are the m x n arrays containing each blade element's lift and
% drag coefficients.
% note that output coefficients will be non-dimensionalized by a constant
% blade chord, typically the average blade chord.
%
% The blade polar function is responsible for capturing all important rotor
% phenomena, including reverse flow, retreating blade stall, and advancing
% tip critical mach number.
%
% to capture reverse flow effects, the drag polar should be able to handle
% alphas +/- 360 deg
%
% critical mach drag rise and stall should also be modeled.

% This file is an example of using wind tunnel data (NACA 0012), very fast
% interpolation, and a critical mach drag rise approximation.

% Drag Coefficient data
%from http://www.cyberiad.net
% -------------------------------- REYNOLDS NUMBER -----------------------

% RE = [5000000] ;

DragData = [    0.0064
    0.0064
    0.0066
    0.0068
    0.0072
    0.0076
    0.0081
    0.0086
    0.0092
    0.0098
    0.0106
    0.0118
    0.0130
    0.0143
    0.0159
    0.0177
    0.0198
    0.0229
    0.1480
    0.2740
    0.2970
    0.3200
    0.3440
    0.3690
    0.3940
    0.4200
    0.4460
    0.4730
    0.5700
    0.7450
    0.9200
    1.0750
    1.2150
    1.3450
    1.4700
    1.5750
    1.6650
    1.7350
    1.7800
    1.8000
    1.8000
    1.7800
    1.7500
    1.7000
    1.6350
    1.5550
    1.4650
    1.3500
    1.2250
    1.0850
    0.9250
    0.7550
    0.5750
    0.4200
    0.3200
    0.2300
    0.1400
    0.0550
    0.0250
    0.0550
    0.1400
    0.2300
    0.3200
    0.4200
    0.5750
    0.7550
    0.9250
    1.0850
    1.2250
    1.3500
    1.4650
    1.5550
    1.6350
    1.7000
    1.7500
    1.7800
    1.8000
    1.8000
    1.7800
    1.7350
    1.6650
    1.5750
    1.4700
    1.3450
    1.2150
    1.0750
    0.9200
    0.7450
    0.5700
    0.4730
    0.4460
    0.4200
    0.3940
    0.3690
    0.3440
    0.3200
    0.2970
    0.2740
    0.1480
    0.0229
    0.0198
    0.0177
    0.0159
    0.0143
    0.0130
    0.0118
    0.0106
    0.0098
    0.0092
    0.0086
    0.0081
    0.0076
    0.0072
    0.0068
    0.0066
    0.0064
    0.0064];
LiftData =[          0
    0.1100
    0.2200
    0.3300
    0.4400
    0.5500
    0.6600
    0.7700
    0.8800
    0.9900
    1.1000
    1.1842
    1.2673
    1.3242
    1.3423
    1.3093
    1.2195
    1.0365
    0.9054
    0.8412
    0.8233
    0.8327
    0.8563
    0.8903
    0.9295
    0.9718
    1.0193
    1.0680
    0.9150
    1.0200
    1.0750
    1.0850
    1.0400
    0.9650
    0.8750
    0.7650
    0.6500
    0.5150
    0.3700
    0.2200
    0.0700
    -0.0700
    -0.2200
    -0.3700
    -0.5100
    -0.6250
    -0.7350
    -0.8400
    -0.9100
    -0.9450
    -0.9450
    -0.9100
    -0.8500
    -0.7400
    -0.6600
    -0.6750
    -0.8500
    -0.6900
    0
    0.6900
    0.8500
    0.6750
    0.6600
    0.7400
    0.8500
    0.9100
    0.9450
    0.9450
    0.9100
    0.8400
    0.7350
    0.6250
    0.5100
    0.3700
    0.2200
    0.0700
    -0.0700
    -0.2200
    -0.3700
    -0.5150
    -0.6500
    -0.7650
    -0.8750
    -0.9650
    -1.0400
    -1.0850
    -1.0750
    -1.0200
    -0.9150
    -1.0680
    -1.0193
    -0.9718
    -0.9295
    -0.8903
    -0.8563
    -0.8327
    -0.8233
    -0.8412
    -0.9054
    -1.0365
    -1.2195
    -1.3093
    -1.3423
    -1.3242
    -1.2673
    -1.1842
    -1.1000
    -0.9900
    -0.8800
    -0.7700
    -0.6600
    -0.5500
    -0.4400
    -0.3300
    -0.2200
    -0.1100
    0];
Alpha =[0
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    30
    35
    40
    45
    50
    55
    60
    65
    70
    75
    80
    85
    90
    95
    100
    105
    110
    115
    120
    125
    130
    135
    140
    145
    150
    155
    160
    165
    170
    175
    180
    185
    190
    195
    200
    205
    210
    215
    220
    225
    230
    235
    240
    245
    250
    255
    260
    265
    270
    275
    280
    285
    290
    295
    300
    305
    310
    315
    320
    325
    330
    333
    334
    335
    336
    337
    338
    339
    340
    341
    342
    343
    344
    345
    346
    347
    348
    349
    350
    351
    352
    353
    354
    355
    356
    357
    358
    359
    360]*pi/180;
modinpcount = 0;
while any(BladeState.alpha(:)<0)
    ind = BladeState.alpha<0;
    BladeState.alpha(ind) = BladeState.alpha(ind)+2*pi;
    modinpcount=modinpcount+1;
end
if modinpcount>2
    disp('crazy stuff going on in bpolar - very very negative alphas')
end
modinpcount = 0;
while any(BladeState.alpha(:)>2*pi)
    ind = BladeState.alpha>2*pi;
    BladeState.alpha(ind) = BladeState.alpha(ind)-2*pi;
    modinpcount=modinpcount+1;
end
if modinpcount>1
    disp('crazy stuff going on in bpolar - very very positive alphas')
end
cd = mylerp1(Alpha,DragData,BladeState.alpha);%,'linear');
cl = mylerp1(Alpha,LiftData,BladeState.alpha);%,'linear');
if any(isnan([cd(:);cl(:)]));
    disp('    check this out')
end
% Data from http://adg.stanford.edu/aa241/drag/dragrise.html
% at Cl of .3
% M-Mcrit:
Mdiff0 =[ -1
    -0.15
    -0.125
    -0.1
    -0.075
    -0.05
    -0.025
    0
    0.025
    0.05
    0.075
    0.1
    0.125
    0.15
    0.175
    0.2
    0.225
    10000.225];

Cdinvisc =[0
    0
    0
    0.000025
    0.00005
    0.00015
    0.0005
    0.002
    0.0052
    0.0103
    0.0173
    0.026
    0.037
    0.048
    0.068
    0.1
    0.148
    20000];

Mcrit = max(.55,-.02002*abs(BladeState.alpha)+.765);
% from ESDU AERO W.00.03.01
% Critical Mach number for high speed aerofoil sections.

cd = cd + mylerp1(Mdiff0,Cdinvisc,abs(BladeState.M)-Mcrit);

%tip loss
ind = BladeState.r>.97;
cl(ind)=cl(ind).*(1-(BladeState.r(ind)-.97)/.03);

%% root cutout
cl(BladeState.r<.15) = 0;
cd(BladeState.r<.15) = .1;

if any([isnan(cl(:)) isnan(cd(:))])
    disp('NaN in bpolar function')
    cd(isnan(cd)) = 1;
    cl(isnan(cl)) = 1;
end

end

function yi = mylerp1(x,y,xi)
% Y = interp1(x,y,X,'linear');

toreshape = size(xi);
xi = xi(:);
yi = lininterp1f(x,y,xi,NaN); %mex .dll function
yi = reshape(yi,toreshape);

end


function Problem = ITERmodify_LB_X0_UB(Problem)
X0 = Problem.x0(:);
LB = Problem.lb(:);
UB = Problem.ub(:);
XLabels = Problem.XLabels;

repeat = true;
while repeat
    disp(' ')
    BOUNDS = {'Index', 'Parameter'};
    for ii = 1:length(X0)
        BOUNDS{ii+1,1} = ii;
        BOUNDS{ii+1,2} = XLabels{ii};
    end
    disp(BOUNDS);
    LB_X0_UB = [LB X0 UB];
    openvar('LB_X0_UB');
    disp (' ');
    disp('[LB X0 UB] available for editing in array editor.')
    disp('Close array editor and type ''return'' in command window when finished editing')
    keyboard;
    LB = LB_X0_UB(:,1); UB = LB_X0_UB(:,3); X0 = LB_X0_UB(:,2);
    disp('LB, X0, and UB overwritten by edited values')
    repeat = false;
    if any(UB<LB)
        disp('*******************')
        disp('UB < LB at indices:')
        disp(find(UB<LB))
        beep
        repeat = true;
    elseif any(X0<LB) || any(X0>UB)
        disp('One or more seed values outside bounds. What would you like to do?')
        k = txtmenu([],'Edit in array editor',...
            'Trim seed to bounds','Expand bounds to seed');
        if k
            lowind = X0<LB; hiind = X0>UB;
            if k == 1
                X0(lowind) = LB(lowind);
                X0(hiind) = UB(hiind);
            elseif k ==2
                LB(lowind) = X0(lowind);
                UB(hiind) = X0(hiind);
            end
        else
            repeat = true;
        end
    end
end

Problem.x0 =    X0;
Problem.lb =    LB;
Problem.ub =    UB;


function [OffDec,OffMask] = Operator(Problem,ParentDec,ParentMask,Fitness)
% The operator of SparseEA2

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    %% Parameter setting
    [N,~]       = size(ParentDec);
    Parent1Dec  = ParentDec(1:floor(end/2),:);
    Parent2Dec  = ParentDec(floor(end/2)+1:floor(end/2)*2,:);
    Parent1Mask = ParentMask(1:floor(end/2),:);
    Parent2Mask = ParentMask(floor(end/2)+1:floor(end/2)*2,:);

     %% Crossover and mutation for dec
     if any(Problem.encoding~=4)
         [OffDec,groupIndex,chosengroups] = GLP_OperatorGAhalf(Problem,Parent1Dec,Parent2Dec,4);	% 4 -- numberofgroups
         OffDec(:,Problem.encoding==4) = 1;
     else
         OffDec = ones(size(Parent1Dec));
     end

    %% Crossover for mask
    OffMask = Parent1Mask;
    for i = 1 : N/2
        if rand < 0.5
            index = find(Parent1Mask(i,:)&~Parent2Mask(i,:));
            index = index(TS(-Fitness(index)));
            OffMask(i,index) = 0;
        else
            index = find(~Parent1Mask(i,:)&Parent2Mask(i,:));
            index = index(TS(Fitness(index)));
            OffMask(i,index) = Parent2Mask(i,index);
        end
    end

    %% Mutation for mask
    if any(Problem.encoding~=4)
        chosenindex = groupIndex == chosengroups;
        for i = 1 : N/2
            if rand < 0.5
                index = find(OffMask(i,:)&chosenindex(i,:));
                index = index(TS(-Fitness(index)));
                OffMask(i,index) = 0;
            else
                index = find(~OffMask(i,:)&chosenindex(i,:));
                index = index(TS(Fitness(index)));
                OffMask(i,index) = 1;
            end
        end
    end
end

function index = TS(Fitness)
% Binary tournament selection
    if isempty(Fitness)
        index = [];
    else
        index = TournamentSelection(2,1,Fitness);
    end
end


%DATASETS Info on the dataset class construction for PRTools
%
% This is not a command, just an information file.
%
% Datasets in PRTools are in the MATLAB language defined as objects of the
% class PRDATASET. Below, the words 'object' and 'class' are used in the pattern
% recognition sense.
%
% A dataset is a set consisting of M objects, each described by K features.
% In PRTools, such a dataset is represented by a M x K matrix: M rows, each
% containing an object vector of K elements. Usually, a dataset is labeled.
% An example of a definition is:
%
%  DATA = [RAND(3,2) ; RAND(3,2)+0.5];
%  LABS = ['A';'A';'A';'B';'B';'B'];
%  A = PRDATASET(DATA,LABS)
%
% which defines a [6 x 2] dataset with 2 classes.
%
% The [6 x 2] data matrix (6 objects given by 2 features) is accompanied by
% labels, assigning each of the objects to one of the two classes A and B.
% Class labels can be numbers or strings and should always be given as rows
% in the label list. A lable may also have the value NaN or may be an empty
% string, indicating an ulabeled object. If the label list is not given,
% all objects are marked as unlabeled.
%
% Various other types of information can be stored in a dataset. The most
% simple way to get an overview is by typing:
%
%  STRUCT(A)
%
% which for the above example displays the following:
%
%         DATA: [6x2 double]
%      LABLIST: [2x1 double]
%         NLAB: [6x1 double]
%      LABTYPE: 'crisp'
%      TARGETS: []
%      FEATLAB: [2x1 double]
%      FEATDOM: {1x2 cell}
%        PRIOR: []
%         COST: []
%      OBJSIZE: 6
%     FEATSIZE: 2
%        IDENT: {6x1 cell}
%      VERSION: {1x2 cell}
%         NAME: []
%         USER: []
%
% These fields have the following meaning:
%
% DATA     : an array containing the objects (the rows) represented by
%            features (the columns). In the software and help-files, the number
%            of objects is usually denoted by M and the number of features is
%            denoted by K. So, DATA has the size of [M,K]. This is also defined
%            as the size of the entire dataset.
% LABLIST  : The names of the classes, stored row-wise. These class names
%            should be integers, strings or cells of strings. Mixtures of
%            these are not supported. LABLIST has as many rows as there are
%            classes. This number is usually denoted by C. LABLIST is
%            constructed from the set of LABELS given in the DATASET command
%            by determining the unique names while ordering them alphabetically.
% NLAB     : an [M x 1] vector of integers between 1 and C, defining for each
%            of the M objects its class. They are indexing LABLIST.
% LABTYPE  : 'CRISP', 'SOFT' or 'TARGETS' are the three possible label types.
%            In case of 'CRISP' labels, a unique class, defined by NLAB, is
%            assigned to each object, pointing to the class names given in
%            LABLIST.
%            For 'SOFT' labels, each object has a corresponding vector of C
%            numbers between 0 and 1 indicating its membership (or confidence
%            or posterior probability) of each of the C classes. These numbers
%            are stored in the array TARGETS of the size M x C. They don't
%            necessarily sum to one for individual row vectors.
%            Labels of type 'TARGETS' are in fact no labels, but merely target
%            vectors of length C. The values are again stored in TARGETS and
%            are not restricted in value.
% TARGETS  : [M,C] array storing the values of the soft labels or targets.
% FEATLAB  : A label list (like LABLIST) of K rows storing the names of the
%            features.
% FEATDOM  : A cell array describing for each feature its domain.
% PRIOR    : Vector of length C storing the class prior probabilities. They
%            should sum to one. If PRIOR is empty ([]) it is assumed that the
%            class prior probabilities correspond to the class frequencies.
% COST     : Classification cost matrix. COST(I,J) are the costs
%            of classifying an object from class I as class J. Column C+1
%            generates an alternative reject class and may be omitted,
%            yielding a size of [C,C]. An empty cost matrix, COST = []
%            (default) is interpreted as COST = ONES(C) - EYE(C) (identical
%            costs of misclassification).
% OBJSIZE  : The number of objects, M. In case the objects are related to a
%            n-dimensional structure, OBJSIZE is a vector of length n, storing
%            the size of this structure. For instance, if the objects are pixels
%            in a [20 x 16] image, then OBJSIZE = [20,16] and M = 320.
% FEATSIZE : The number of features, K. In case the features are related to
%            an n-dimensional structure, FEATSIZE is a vector of length n,
%            storing the size of this structure. For instance, if the features
%            are pixels in a [20 x 16] image, then FEATSIZE = [20,16] and
%            K = 320.
% IDENT    : A cell array of M elements storing indicators of the M objects.
%            They are initialized by integers 1:M.
% VERSION  : Some information related to the version of PRTools used for
%            defining the dataset.
% NAME     : A character string naming the dataset, possibly used to annotate
%            related graphics.
% USER     : Free field for the user, not used by PRTools.
%
%
% The fields can be set by commands like SETDATA, SETFEATLAB, SETLABELS,
% see below for a complete list.
% Note that there is no field LABELS in the DATASET definition. Labels are
% converted to NLAB and LABLIST. The command SETLABELS however exists and
% takes care of the conversion.
%
% The data and information stored in a dataset can be retrieved as follows:
%
% - By DOUBLE(A) and by +A, the content of A.DATA is returned.
% - [N,LABLIST] = CLASSSIZES(A);
%   It returns the numbers of objects per class and the class names stored
%   in LABLIST.
% - By DISPLAY(A), it writes the size of the dataset, the number of classes
%   and the label type on the terminal screen.
% - By SIZE(A), it returns the size of A.DATA: numbers of objects and features.
% - By SCATTERD(A), it makes a scatter plot of a dataset.
% - By SHOW(A), it may be used to display images that are stored as features
%   or as objects in a dataset.
% - By commands like: GETDATA, GETFEATLAB, etcetera, see below. With some
%   exceptions they point to a single dataset field. E.g. GETSIZE(A) returns
%   [M,K,C]. A aet of commands does not return data, but instead they return
%   indices to objects that have specific identifiers, labels or class indices:
%   FINDIDENT, FINDLABELS, FINDNLAB.
%
% Many standard MATLAB operations and a number of general MATLAB commands have
% been overloaded for variables of the DATASET type.
%
% SEE ALSO (PRTools Guide)
% PRDATASET, DATA2IM, OBJ2FEAT, FEAT2OBJ, IM2FEAT, IM2OBJ, DATAIM
% SETDATA, SETFEATLAB, SETFEATDOM, SETFEATSIZE, SETIDENT, SETLABELS,
% SETLABLIST, SETLABTYPE, SETNAME, SETNLAB, SETOBJSIZE, SETPRIOR, SETCOST,
% SETTARGETS, SETUSER, SETLABLISTNAMES, SETVERSION
% GETDATA, GETFEATLAB, GETFEATDOM, GETFEATSIZE, GETIDENT, GETLABELS,
% GETLABLIST, GETLABTYPE, GETNAME, GETNLAB, GETOBJSIZE, GETPRIOR, GETCOST,
% GETSIZE, GETTARGETS, GETUSER, GETVERSION, GETCLASSI, GETLABLISTNAMES,
% FINDIDENT, FINDLABELS, FINDNLAB

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands


%Program for random permuting the share generation

%Author : Athi Narayanan S
%M.E, Embedded Systems,
%K.S.R College of Engineering
%Erode, Tamil Nadu, India.
%http://sites.google.com/site/athisnarayanan/
%s_athi1983@yahoo.co.in

%Program Description
%This program generates the share 1 & share 2 randomly for each pixel.

function out = generateShare(a,b)

a1 = a(1);
a2 = a(2);
b1 = b(1);
b2 = b(2);

in = [a
      b];
out = zeros(size(in));
randNumber = floor(1.9*rand(1));

if (randNumber == 0)
    out = in;
elseif (randNumber == 1)
    a(1) = a2;
    a(2) = a1;
    b(1) = b2;
    b(2) = b1;
    out = [a
           b];
end