function[varargout]=about_jtopo(varargin)
%ABOUT_JTOPO  One-sixth degree global topography, from Smith and Sandwell + IBCAO.
%   _______________________________________________________________________
%
%   *|* jtopo.png --- Figure illustrating the JTOPO topography dataset.
%   Type 'jhelp about_jtopo' to view this image. *|*
%   _______________________________________________________________________
%
%   JTOPO is a matfile containing smoothed one-twelfth degree global
%   topography based on the Smith and Sandwell database together with the
%   International Bathymetric Chart of the Arctic Ocean (IBCAO).
%
%   LOAD JTOPO loads the structure JTOPO, with the following fields:
%
%       jtopo.about     Pointer to this document
%       jtopo.lat       Array of latitudes         [2048 x 1]
%       jtopo.lon       Array of longitudes        [1 x 4320]
%       jtopo.topo      Matrix of topography       [2048 x 4320]
%
%   Typing 'use jtopo' maps these fields into variables in the current
%   workspace, e.g. 'use jtopo, pcolor(lon,lat,topo), shading interp'.
%
%   TOPO is in units of kilometers and is positive for above sea level,
%   and negative for below sea level.
%
%   LAT is uniformly spaced from -80.666 to  89.917, and LON is uniformly
%   spaced from -180 to 179.912.  These are *grid-centered* values, that
%   is, they indicate midpoints of the topography cells.
%
%   JTOPO is distributed with JLAB, available at http://www.jmlilly.net.
%
%   See also READTOPO, which reads in any region of the Smith and Sandwell
%   data, ABOUT_IBCAO, and TOPOPLOT, which makes simple plots using JTOPO.
%   __________________________________________________________________
%
%   Processing
%
%   The one-minute Smith and Sandwell data, and 1/2 minute IBCAO data, are
%   smoothed to one-twelfth of a degree by averaging in 1/12 x 1/12 bins.
%
%   Smith and Sandwell is defined from -80.738 and 80.738, while IBCAO is
%   defined from 64 N to 90 N.  In the overlap region, from 64 to 84.738 N,
%   the root-mean-square different between the two datasets is about 50 m.
%
%   JTOPO uses a linear blend to resolve the small discrepencies in the
%   overlap region.  From 64 to 84.738 N, JTOPO transitions from being all
%   Smith and Sandwell, to all IBCAO, with a weighted average in between.
%   __________________________________________________________________
%
%   Data and documentation
%
%   This dataset is based on the Smith and Sandwell Global Topography
%   Dataset v. 19.1 and IBCAO v. 3.0, which are included with JDATA.
%
%   The source and reference for the Smith and Sandwell dataset are
%
%       http://topex.ucsd.edu/WWW_html/mar_topo.html
%
%      Smith, W. H. F., and D. T. Sandwell, Global seafloor topography
%         from satellite altimetry and ship TOPO soundings, Science,
%         v. 277, p. 1957-1962, 26 Sept., 1997.
%
%   The source and reference for the IBCAO dataset are
%
%      http://www.ngdc.noaa.gov/mgg/bathymetry/arctic/grids/version3_0/
%
%      Jakobsson, M., L. A. Mayer, B. Coakley, J. A. Dowdeswell, S. Forbes,
%          B. Fridman, H. Hodnesdal, R. Noormets, R. Pedersen, M. Rebesco,
%          H.-W. Schenke, Y. Zarayskaya A, D. Accettella, A. Armstrong,
%          R. M. Anderson, P. Bienhoff, A. Camerlenghi, I. Church,
%          M. Edwards, J. V. Gardner, J. K. Hall, B. Hell, O. B. Hestvik,
%          Y. Kristoffersen, C. Marcussen, R. Mohammad, D. Mosher,
%          S. V. Nghiem, M. T. Pedrosa, P. G. Travaglini, and
%          P. Weatherall, The International Bathymetric Chart of the Arctic
%          Ocean (IBCAO) Version 3.0, Geophysical Research Letters,
%          doi: 10.1029/2012GL052219
%   __________________________________________________________________
%
%   License and Copyright
%
%   JTOPO.MAT is distributed with JDATA for RESEARCH AND NON-PROFIT USE
%   ONLY, in accordance with the copyright statement for the Smith and
%   Sandwell dataset.  For details, see TOPO_COPYRIGHT.
%
%   No copyright or policy is specified in the IBCAO documentation.
%   __________________________________________________________________
%
%   Dataset creation
%
%   For completeness, the m-file ABOUT_JTOPO also contains the processing
%   steps used in the creation of JTOPO.MAT.
%
%   If you wish to do this yourself, with JLAB on your search path,
%   'about_jtopo --create' will recreate the JTOPO.MAT dataset by reading
%   in and averaging the two topographic datasets. This will take a while.
%
%   For this to work you will need to have the JDATA folder containing
%   file 'topo_19.1.img' and 'ibcao.mat' downloaded and on your Matlab
%   search path.
%   __________________________________________________________________
%
%   See also READTOPO, TOPOPLOT, JDATA.
%
%   'about_jtopo --f' generates the sample figure shown above.
%
%   Usage: about_jtopo
%          about_jtopo --create
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2014--2020 J.M. Lilly --- type 'help jlab_license' for details

if nargin==0
    help about_jtopo
elseif nargin>0
    if strcmpi(varargin{1}, '--create')
        jtopo_create,return
    elseif strcmpi(varargin{1}, '--f')
        type makefigs_jtopo
        makefigs_jtopo;
        return
    end
end

function[]=jtopo_create

%/************************************************************************
%Main Smith and Sandwell portion
%delta=1/6;
delta=1/12;

lon=[-180-delta/2:delta:180-delta/2];
lat=[-80.66666-delta/2:delta:80.67+delta/2]';

topo=zeros(length(lat)-1,length(lon)-1);
for i=1:length(lon)-1
    disp(['Longitude band number ' num2str(i) ' of ' num2str(length(lon)-1) '.' ])
    [topo1,lat1,lon1]=readtopo([lon(i) lon(i)+delta -80.738 80.738]);
    topo1=vmean(topo1,2);  %Average across longitudes
    [mz,latbin,lonbin]=twodstats(lat1,lon(i)+0*lat1+delta/2,topo1,lat,[lon(i) lon(i)+delta]);%Average across latitudes
    topo(:,i)=mz';
end

lat=latbin;
lon=lon(1:end-1)+delta/2;

about='For more information, type ''about_jtopo''.';
matsave jtopo about lat lon topo
%matsave jtopo_onetwelfth about lat lon topo
%\************************************************************************

%/************************************************************************
%Adding Arctic dataset
lon=[-180-delta/2:delta:180-delta/2];
lat=[-80.66666-delta/2:delta:90]';

newtopo=zeros(length(lat)-1,length(lon)-1);
newtopo(1:size(jtopo.topo,1),:)=jtopo.topo;

load ibcao
ibcaotopo=zeros(size(newtopo));
[long,latg]=meshgrid(ibcao.lon,ibcao.lat);
for i=1:length(lon)-1
    disp(['Longitude band number ' num2str(i) ' of ' num2str(length(lon)-1) '.' ])
    index=find(ibcao.lon>lon(i) & ibcao.lon< lon(i)+delta);
    [mz,latbin,lonbin]=twodstats(latg(:,index),long(:,index),ibcao.topo(:,index),...
        lat(min(find(lat>64)):end),[lon(i) lon(i)+delta]);%Average across latitudes
    ibcaotopo(min(find(lat>64)):end,i)=mz';
end

%topodiff=jtopo.topo(min(find(lat>64)):end,:)-ibcaotopo(1:100,:);
%figure,plot(1000*vmean(abs(topodiff),2))
%sqrt(mean(squared(topodiff(:))))*1000 = 52 meters
lat=lat(1:end-1)+delta/2;
lon=lon(1:end-1)+delta/2;

%Linearly blend between the two datasets
index=min(find(lat>64)):find(lat<max(jtopo.lat),1,'last');
weight=ones(size(lat));
weight(index)=1-[0:length(index)-1]./(length(index)-1);
weight(index(end)+1:end)=0;

%figure,plot(lat,weight),vlines([64 80.6666666])
weight=vrep(weight,size(newtopo,2),2);
topo=newtopo.*weight+ibcaotopo.*(1-weight);
%\************************************************************************

matsave jtopo about lat lon topo
%matsave jtopo_onetwelfth about lat lon topo
disp('JTOPO creation complete.')


function coords = roiGrowCoords(ui);
% Grow Coords from a seed in mrViewer.
%
% coords = roiGrowCoords(ui);
%
% Get a point from the user from which to grow a 'blob' of
% interconnected points in 3D, and return a 3xN list of coordinates
% for those points (in the base MR data).
% ras 09/2005.
figure(ui.fig);
tmp = get(ui.panels.display,'BackgroundColor');
set(ui.panels.display,'BackgroundColor','y');
hmsg = mrMessage('Select point from which to grow.');
pt = round(ginput(1));
close(hmsg);
set(ui.panels.display,'BackgroundColor',tmp);

% figure out what image was clicked on
imgNum = find(ui.display.axes==gca);
if isempty(imgNum)
    warning('Didn''t click on mrViewer display. No points added.')
    coords = [];
    return
end

% find seed location in the data coords
imgCoords = mrViewGet(ui,'DataCoords',imgNum);
imgSz = size(ui.display.images{imgNum});
ind = sub2ind(imgSz,pt(2),pt(1));
seed = round(imgCoords(:,ind));

% First-pass method of growing a blob efficiently:
% (1) Create an initial box, centered on seed
% (2) Restrict the box to coords which pass overlay thresholds
% (3) Find contiguous blob which contains seed
% (4) Test if box contains whole blob: if not, repeat 1-4 iteratively
hmsg = msgbox('Growing ROI...');
boxSize = 10; stepSize = 10;
coords = [];
while 1
    % (1) create box (get coords specifying box in data)
    ymin = max(1,seed(1)-boxSize);
    ymax = min(ui.mr.dims(1),seed(1)+boxSize);
    xmin = max(1,seed(2)-boxSize);
    xmax = min(ui.mr.dims(2),seed(2)+boxSize);
    zmin = max(1,seed(3)-boxSize);
    zmax = min(ui.mr.dims(3),seed(3)+boxSize);
    [X Y Z] = meshgrid(xmin:xmax,ymin:ymax,zmin:zmax);
    boxCoords = [Y(:) X(:) Z(:)]'; clear X Y Z
    mask = logical(zeros(ui.mr.dims(1:3)));

    % (2) restrict box to overlays
    boxCoords = mrViewRestrict(ui,boxCoords);
    ok = sub2ind(size(mask),boxCoords(1,:),boxCoords(2,:),boxCoords(3,:));
    mask(round(ok)) = 1;

    % (3) Find contiguous blob which contains seed
    L = bwlabeln(mask, 6); % integer label matrix of 6-connected blobs
    seedLabel = L(seed(1),seed(2),seed(3));
    if seedLabel==0, return;   end;  % no data so quit
    blob = (L==seedLabel); % binary matrix where blob is 1
    [i1, i2, i3] = ind2sub(size(mask),find(blob>0.5));
    coords = [i1 i2 i3]';

    % (4) Test if the box contains the entire blob
    % (Or is otherwise flush with the bounds of the data)
    inBoundsY=(all(i1>ymin)|ymin==1) & (all(i1<ymax)|ymax==ui.mr.dims(1));
    inBoundsX=(all(i2>xmin)|xmin==1) & (all(i2<xmax)|xmax==ui.mr.dims(2));
    inBoundsZ=(all(i3>zmin)|zmin==1) & (all(i3<zmax)|zmax==ui.mr.dims(3));
    if inBoundsX & inBoundsY & inBoundsZ
        break;
    else
        boxSize = boxSize + stepSize;
    end
end
close(hmsg);

return


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

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defun findsymbols (@var{x})
%% Return a list (cell array) of the symbols in an expression.
%%
%% The list is sorted alphabetically.  For details, @pxref{@@sym/symvar}.
%%
%% If two variables have the same symbol but different assumptions,
%% they will both appear in the output.  It is not well-defined
%% in what order they appear.
%%
%% @var{x} could be a sym, sym array, cell array, or struct.
%%
%% @example
%% @group
%% syms x y z
%% C = @{x, 2*x*y, [1 x; sin(z) pi]@};
%% S = findsymbols (C)
%%   @result{} S = @{ ... @}
%% S@{:@}
%%   @result{} ans = (sym) x
%%   @result{} ans = (sym) y
%%   @result{} ans = (sym) z
%% @end group
%% @end example
%%
%% Note ℯ, ⅈ, π, etc are not considered as symbols.
%%
%% Note only returns symbols actually appearing in the RHS of a
%% @code{symfun}.
%%
%% @seealso{symvar, @@sym/symvar, @@sym/findsym}
%% @end defun

function L = findsymbols(obj, dosort)

  if (nargin == 1)
    dosort = true;
  elseif (nargin ~= 2)
    print_usage ();
  end

  if isa(obj, 'sym')
    cmd = { 'if isinstance(_ins[0], MatrixBase):'
            '    # having problems with Eq in matrices in SymPy >= 1.7.1'
            '    s = set().union(*(i.free_symbols for i in _ins[0] if i is not None))'
            'else:'
            '    s = _ins[0].free_symbols'
            'l = list(s)'
            'l = sorted(l, key=str)'
            'return l,' };
    L = pycall_sympy__ (cmd, obj);

  elseif iscell(obj)
    %fprintf('Recursing into a cell array of numel=%d\n', numel(obj))
    L = {};
    for i=1:numel(obj)
      temp = findsymbols(obj{i}, false);
      if ~isempty(temp)
        L = {L{:} temp{:}};
      end
    end

  elseif isstruct(obj)
    %fprintf('Recursing into a struct array of numel=%d\n', numel(obj))
    L = {};
    fields = fieldnames(obj);
    for i=1:numel(obj)
      for j=1:length(fields)
        thisobj = getfield(obj, {i}, fields{j});
        temp = findsymbols(thisobj, false);
        if ~isempty(temp)
          L = {L{:} temp{:}};
        end
      end
    end

  else
    L = {};
  end

  % sort and make unique using internal representation
  if dosort
    Ls = {};
    for i=1:length(L)
      Ls{i} = sympy (L{i});
    end
    [tilde, I] = unique(Ls);
    L = L(I);
  end
end

%!test
%! syms x b y n a arlo
%! z = a*x + b*pi*sin (n) + exp (y) + exp (sym (1)) + arlo;
%! s = findsymbols (z);
%! assert (isequal ([s{:}], [a,arlo,b,n,x,y]))
%!test
%! syms x
%! s = findsymbols (x);
%! assert (isequal (s{1}, x))
%!test
%! syms z x y a
%! s = findsymbols ([x y; 1 a]);
%! assert (isequal ([s{:}], [a x y]))
%!assert (isempty (findsymbols (sym (1))))
%!assert (isempty (findsymbols (sym ([1 2]))))
%!assert (isempty (findsymbols (sym (nan))))
%!assert (isempty (findsymbols (sym (inf))))
%!assert (isempty (findsymbols (exp (sym (2)))))

%!test
%! % empty sym for findsymbols, findsym, and symvar
%! assert (isempty (findsymbols (sym([]))))
%! assert (isempty (findsym (sym([]))))
%! assert (isempty (symvar (sym([]))))

%!test
%! % diff. assumptions make diff. symbols
%! x1 = sym('x');
%! x2 = sym('x', 'positive');
%! f = x1*x2;
%! assert (length (findsymbols (f)) == 2)

%!test
%! % symfun or sym
%! syms x f(y)
%! a = f*x;
%! b = f(y)*x;
%! assert (isequal (findsymbols(a), {x y}))
%! assert (isequal (findsymbols(b), {x y}))

%!test
%! % findsymbols on symfun does not find the argnames (unless they
%! % are on the RHS of course, this matches SMT 2014a).
%! syms a x y
%! f(x, y) = a;  % const symfun
%! assert (isequal (findsymbols(f), {a}))
%! syms a x y
%! f(x, y) = a*y;
%! assert (isequal (findsymbols(f), {a y}))

%!test
%! % sorts lexigraphically, same as symvar *with single input*
%! % (note symvar does something different with 2 inputs).
%! syms A B a b x y X Y
%! f = A*a*B*b*y*X*Y*x;
%! assert (isequal (findsymbols(f), {A B X Y a b x y}))
%! assert (isequal (symvar(f), [A B X Y a b x y]))

%!test
%! % symbols in matpow
%! syms x y
%! syms n
%! A = [sin(x) 2; y 1];
%! B = A^n;
%! L = findsymbols(B);
%! assert (isequal (L, {n x y}))

%!test
%! % array of eq
%! syms x y
%! assert (isequal (findsymbols (2 == [2 x y]), {x y}))

%!test
%! % array of ineq
%! syms x y
%! A = [x < 1  2*x < y  x >= 2  3 <= x  x != y];
%! assert (isequal (findsymbols (A), {x y}))


function i1mach_test ( )

%*****************************************************************************80
%
%% I1MACH_TEST reports the constants returned by I1MACH.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 April 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'I1MACH_TEST\n' );
  fprintf ( 1, '  I1MACH reports the value of constants associated\n' );
  fprintf ( 1, '  with integer computer arithmetic.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Numbers associated with input/output units:\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  I1MACH(1) = the standard input unit.\n' );
  fprintf ( 1, '%d\n', i1mach(1) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  I1MACH(2) = the standard output unit.\n' );
  fprintf ( 1, '%d\n', i1mach(2) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  I1MACH(3) = the standard punch unit.\n' );
  fprintf ( 1, '%d\n', i1mach(3) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  I1MACH(4) = the standard error message unit.\n' );
  fprintf ( 1, '%d\n', i1mach(4) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Numbers associated with words:\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  I1MACH(5) = the number of bits per integer.\n' );
  fprintf ( 1, '%d\n', i1mach(5) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  I1MACH(6) = the number of characters per integer.\n' );
  fprintf ( 1, '%d\n', i1mach(6) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Numbers associated with integer values:\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Assume integers are represented in the S digit \n' );
  fprintf ( 1, '  base A form:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Sign * (X(S-1)*A^(S-1) + ... + X(1)*A + X(0))\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  where the digits X satisfy 0 <= X(1:S-1) < A.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  I1MACH(7) = A, the base.\n' );
  fprintf ( 1, '%d\n', i1mach(7) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  I1MACH(8) = S, the number of base A digits.\n' );
  fprintf ( 1, '%d\n', i1mach(8) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  I1MACH(9) = A^S-1, the largest integer.\n' );
  fprintf ( 1, '%d\n', i1mach(9) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Numbers associated with floating point values:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Assume floating point numbers are represented \n' );
  fprintf ( 1, '  in the T digit base B form:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Sign * (B^E) * ((X(1)/B) + ... + (X(T)/B^T) )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  where \n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    0 <= X(1:T) < B,\n' );
  fprintf ( 1, '    0 < X(1) (unless the value being represented is 0),\n' );
  fprintf ( 1, '    EMIN <= E <= EMAX.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  I1MACH(10) = B, the base.\n' );
  fprintf ( 1, '%d\n', i1mach(10) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Numbers associated with single precision values:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  I1MACH(11) = T, the number of base B digits.\n' );
  fprintf ( 1, '%d\n', i1mach(11) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  I1MACH(12) = EMIN, the smallest exponent E.\n' );
  fprintf ( 1, '%d\n', i1mach(12) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  I1MACH(13) = EMAX, the largest exponent E.\n' );
  fprintf ( 1, '%d\n', i1mach(13) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Numbers associated with double precision values:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  I1MACH(14) = T, the number of base B digits.\n' );
  fprintf ( 1, '%d\n', i1mach(14) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  I1MACH(15) = EMIN, the smallest exponent E.\n' );
  fprintf ( 1, '%d\n', i1mach(15) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  I1MACH(16) = EMAX, the largest exponent E.\n' );
  fprintf ( 1, '%d\n', i1mach(16) );

  return
end


function x = cs_usolve (U,b)                                                %#ok
%CS_USOLVE solve a sparse upper triangular system U*x=b.
%   x = cs_usolve(U,b) computes x = U\b, U must be lower triangular with a
%   zero-free diagonal.  b must be a column vector.  x is full if b is full.
%   If b is sparse, x is sparse but nonzero pattern of x is NOT sorted (it is
%   returned in topological order).
%
%   Example:
%       Prob = UFget ('HB/arc130') ; A = Prob.A ; n = size (A,1) ;
%       b = rand (n,1);
%       [L U p q] = cs_lu (A) ;
%       x = cs_usolve (U, cs_lsolve (L, b(p))) ;   % x = U \ (L \ b(p)) ;
%       x (q) = x ;
%       norm (A*x-b)
%
%   See also CS_LSOLVE, CS_LTSOLVE, CS_UTSOLVE, MLDIVIDE.

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

error ('cs_usolve mexFunction not found') ;


function data = readdata(filename, m)
%-------------------------------------------------------------------------------
% data = readdata(filename, m)
% read binary data with m double
% by Dr. Yudan Yi
%-------------------------------------------------------------------------------
data = [];
if (nargin<2) return; end;
if (isempty(m)|m<=0) return; end;
fdat = fopen(filename, 'rb');
if (fdat==-1) return; end;
data = fread(fdat, 'double');
n = size(data,1);
nn = floor(n/m);
data = reshape(data(1:nn*m), m, nn)';
data = sortrows(data, 1);
loc = find(diff(data(:,1))==0);
data(loc, :) = [];
fclose(fdat);
%-------------------------------------------------------------------------------


function[y] = expand(x)

[N,M] = size(x);
N = N*2;
M = M*2;

y = zeros(N,M);
y(1:2:N,1:2:M) = x;
y(2:2:N,2:2:M) = x;
y(1:2:N,2:2:M) = x;
y(2:2:N,1:2:M) = x;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% vertex.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [n0,x,y,x1,x2,f1,f2] = vertex(j,n,u,v,v1,x0,f0,ipar,isplit,
% ichild,z,f,l,L)
% computes the base vertex x and the opposite vertex y of the box # j
% of MCS and the 'neighboring vertices' x1 and x2 and their function
% values f1 and f2 needed for separable quadratic interpolation
% Input:
% j       label of the box whose vertices etc. are to be computed
%         (in the main program mcs.m)
% n       dimension of the problem
% [u,v]   original box
% v1      opposite vertex of the original box
% x0(1:n,1:max(L)) coordinates used in the initialization list
%         x^0 = (x0(1,l(1)),...,x0(n,l(n)) base vertex of the original
%	  box
% f0(1:max(L),1:n) function values appertaining to the initialization
%	  list
%         f0(m,i) is the function value at x = (x0(1,istar(1)),...,
%         x0(i-1,istar(i-1)),x0(i,m),x0(i+1,l(i+1)),...,x0(n,l(n)))
% f0(1:max(L),m), m > n  function values appertaining to a later box
%         split according to the initialization list
% ipar(m) label of the parent of box m
% isplit(m) = -(splitting index) of box m  if it is split in the
%             initialization procedure
%           = splitting index of box m  otherwise
% ichild(m) = -(number which child box m is)  if box m was generated
%             by splitting according to the initialization list (in
%             the initialization procedure or later)
%           = number which child box m is  otherwise
% z(1:2,:)  z(1,m) = value of the isplit(m)th coordinate of the base
%           vertex of box m
%           z(2,m) = k  if box m is split according to the
%                       initialization list and f0(:,k) contains the
%                       function values obtained by splitting the box
%           z(2,m) = splitting value of box m  otherwise
% f(1:2,:)  f(1,m) = function value at the base vertex of box m
%           f(2,m) = function value at the splitting point of box m
%                    (if z(2,m) ~= Inf)
% l(1:n)  pointer defining the initial base vertex x^0 (see above)
% L(1:n)  the init. list contains L(i) values for coordinate i
% Output:
% n0(1:n)  n0(i) indicates that the ith coordinate has been split n0(i)
%          times in the history of box j
% x(1:n)   base vertex of box j
% y(1:n)   opposite vertex of box j
% x1(1:n), x2(1:n), f1(1:n), f2(1:n)
%          x1(i) and x2(i) and the corresponding function values f1(i)
%          and f2(i) are used for quadratic interpolation in the ith
%          coordinate
% Since it is not always possible to find for each i two vertices that
% differ from x only in coordinate i, we have to generate 'fictitious'
% vertices and function values using the assumption that the function
% is separable
% Uses the following functions/m-files:
% updtf.m
% vert1.m
% vert2.m
% vert3.m

function [n0,x,y,x1,x2,f1,f2] = vertex(j,n,u,v,v1,x0,f0,ipar,isplit,ichild,z,f,l,L)
% initialization
% The coordinates of x, y, x1 and x2 are initially set to Inf to
% indicate that these quantities haven't been found yet in the course of
% pursuing the history of box j
x = Inf*ones(n,1);
y = Inf*ones(n,1);
x1 = Inf*ones(n,1);
x2 = Inf*ones(n,1);
f1 = zeros(n,1);
f2 = zeros(n,1);
n0 = zeros(n,1);
fold = f(1,j);
m = j;
while m > 1
  i = abs(isplit(ipar(m)));
  n0(i) = n0(i) + 1;
  if ichild(m) == 1
    if x(i) == Inf | x(i) == z(1,ipar(m))
      [x(i),x1(i),x2(i),f1(i),f2(i)] = vert1(2,z(:,ipar(m)),f(:,ipar(m)),x1(i),x2(i),f1(i),f2(i));
    else
      [f1,f2,fold] = updtf(n,i,x1,x2,f1,f2,fold,f(1,ipar(m)));
      [x1(i),x2(i),f1(i),f2(i)] = vert2(1,x(i),z(:,ipar(m)),f(:,ipar(m)),x1(i),x2(i),f1(i),f2(i));
    end
  elseif ichild(m) >= 2
    [f1,f2,fold] = updtf(n,i,x1,x2,f1,f2,fold,f(1,ipar(m)));
    if x(i) == Inf | x(i) == z(2,ipar(m))
      [x(i),x1(i),x2(i),f1(i),f2(i)] = vert1(1,z(:,ipar(m)),f(:,ipar(m)),x1(i),x2(i),f1(i),f2(i));
    else
      [x1(i),x2(i),f1(i),f2(i)] = vert2(2,x(i),z(:,ipar(m)),f(:,ipar(m)),x1(i),x2(i),f1(i),f2(i));
    end
  end
  if 1 <= ichild(m) & ichild(m) <= 2 & y(i) == Inf
    y(i) = split1(z(1,ipar(m)),z(2,ipar(m)),f(1,ipar(m)),f(2,ipar(m)));
  end
  if ichild(m) < 0
    % box m was generated by splitting according to the init. list
    % x0(i,j1) = ith coordinate of the base vertex of box m
    % the ith coordinate of the opposite vertex is the golden section
    % split of x0(i,j2) and x0(i,j2+1) for 1 <= j2 <= L(i) - 1, it is
    % u(i) for j1 = 1 and v(i) for j2 = L(i)
    % x0(i,j1+j3) = ith coordinate of the 'neighboring vertex'
    if u(i) < x0(i,1)
      j1 = ceil(abs(ichild(m))/2);
      j2 = floor(abs(ichild(m))/2);
      if (abs(ichild(m))/2 < j1  & j1 > 1) | j1 == L(i)
        j3 = -1;
      else
        j3 = 1;
      end
    else
      j1 = floor(abs(ichild(m))/2) + 1;
      j2 = ceil(abs(ichild(m))/2);
      if abs(ichild(m))/2 + 1 > j1 & j1 < L(i)
        j3 = 1;
      else
        j3 = -1;
      end
    end
    if isplit(ipar(m)) < 0 % box m was generated in the init. procedure
      k = i;
    else
      k = z(2,ipar(m));
      % box m was generated by a later split according to the init.
      % list; k points to the corresponding function values
    end
    if j1 ~= l(i) | (x(i) ~= Inf & x(i) ~= x0(i,l(i)))
      [f1,f2,fold] = updtf(n,i,x1,x2,f1,f2,fold,f0(l(i),k));
    end
    if x(i) == Inf | x(i) == x0(i,j1)
      x(i) = x0(i,j1);
      if x1(i) == Inf
        [x1(i),x2(i),f1(i),f2(i)] = vert3(j1,x0(i,:),f0(:,k),L(i),x1(i),x2(i),f1(i),f2(i));
      elseif x2(i) == Inf & x1(i) ~= x0(i,j1+j3)
        x2(i) = x0(i,j1+j3);
        f2(i) = f2(i) + f0(j1+j3,k);
      elseif x2(i) == Inf
        if j1 ~= 1 & j1 ~= L(i)
          x2(i) = x0(i,j1-j3);
          f2(i) = f2(i) + f0(j1-j3,k);
        else
          x2(i) = x0(i,j1+2*j3);
          f2(i) = f2(i) + f0(j1+2*j3,k);
        end
      end
    else
      if x1(i) == Inf
        x1(i) = x0(i,j1);
        f1(i) = f1(i) + f0(j1,k);
        if x(i) ~= x0(i,j1+j3)
          x2(i) = x0(i,j1+j3);
          f2(i) = f2(i) + f0(j1+j3,k);
        end
      elseif x2(i) == Inf
        if x1(i) ~= x0(i,j1)
          x2(i) = x0(i,j1);
          f2(i) = f2(i) + f0(j1,k);
        elseif x(i) ~= x0(i,j1+j3)
          x2(i) = x0(i,j1+j3);
          f2(i) = f2(i) + f0(j1+j3,k);
        else
          if j1 ~= 1 & j1 ~= L(i)
            x2(i) = x0(i,j1-j3);
            f2(i) = f2(i) + f0(j1-j3,k);
          else
            x2(i) = x0(i,j1+2*j3);
            f2(i) = f2(i) + f0(j1+2*j3,k);
          end
        end
      end
    end
    if y(i) == Inf
      if j2 == 0
        y(i) = u(i);
      elseif j2 == L(i)
        y(i) = v(i);
      else
        y(i) = split1(x0(i,j2),x0(i,j2+1),f0(j2,k),f0(j2+1,k));
      end
    end
  end
  m = ipar(m);
end
for i = 1:n
  if x(i) == Inf
    x(i) = x0(i,l(i));
    [x1(i),x2(i),f1(i),f2(i)] = vert3(l(i),x0(i,:),f0(:,i),L(i),x1(i),x2(i),f1(i),f2(i));
  end
  if y(i) == Inf
     y(i) = v1(i);
  end
end


function [pval,rho] = SStat_mass_PHTest(X,d,t,Y,stats_PH,maskvtx)
% [pval,rho] = SStat_mass_PHTest(X,d,t,Y,stats_PH,maskvtx)
%
% Vertex/voxel-wise Schoenfeld residuals test for the proportional hazards
% assumption.
%
% Input
% X: Design Matrix with the time-independent covariates. (mxp, m # of
% subjects, p # of covariates).
% d: Logical vector (mx1) indicating censorship status (1 if the subject got
% the failure event or 0 otherwise).
% t: Vector (mx1) whose entries are the survival and censored times (ordered
% according to X).
% Y: Data matrix (mxnv, nv #vertices) whos colums represent the ordered
% data vector (according to X) for each voxel/vertex.
% stats_PH: Structure array containing statistics obtained from SStat_mass_CoxPH.
% maskvtx: Mask's vertices (1-based). Default [] (all vertices included).
%
% Output
% pval: Matrix of p-values (pxnv) indicating the probability of the PH
% assumption for each covariate and at each location.
% rho: Matrix of correlations (pxnv) between Schoenfeld residuals and ranked
% failure times at each location.
%
% $Revision: 1.1 $  $Date: 2015/01/06 17:03:59 $
% Original Author: Jorge Luis Bernal Rusiel
% CVS Revision Info:
%    $Author: mreuter $
%    $Date: 2015/01/06 17:03:59 $
%    $Revision: 1.1 $
% References: Kleinbaum, D.G., Klein, M., 2005. Survival analysis. A self-
% learning approach, second edition. New York: Springer..
%
%Validation of inputs
if nargin < 5
    error('Too few inputs');
elseif nargin < 6
    maskvtx = [];
end;
nv0 = size(Y,2);
if (length(stats_PH)~=nv0)
    error(['The structure array stats_PH must have the same length as the'...
        ' number of colums of Y.']);
end;
if isempty(maskvtx)
    maskvtx = 1:nv0;
end;
Y = Y(:,maskvtx);
nv = size(Y,2);
[m,p] = size(X);
if (length(d)~=m) || (length(t)~=m) || (size(Y,1)~=m)
    error(['The design matrix X, censorship status vector d, time'...
        ' vector t and data matrix Y must all have the same number of rows.']);
end;
%Sort the data by time. If there is a tie between a failure time and a
%censored time then the failure time goes first.
st_ix = find(d==1);
t1 = t(st_ix);
[t1,t1_ix] = sort(t1);
X1 = X(st_ix(t1_ix),:);
Y1 = Y(st_ix(t1_ix),:);
cs_ix = find(d==0);
if ~isempty(cs_ix)
    t2 = t(cs_ix);
    [t2,t2_ix] = sort(t2);
    X2 = X(cs_ix(t2_ix),:);
    Y2 = Y(cs_ix(t2_ix),:);
    count1 = 1; count2 = 1; i = 0;
    while (count1 <= length(t1)) && (count2 <= length(t2))
        i = i + 1;
        if t1(count1) <= t2(count2)
            X(i,:) = X1(count1,:);
            Y(i,:) = Y1(count1,:);
            d(i) = 1;
            t(i) = t1(count1);
            count1 = count1 + 1;
        else
            X(i,:) = X2(count2,:);
            Y(i,:) = Y2(count2,:);
            d(i) = 0;
            t(i) = t2(count2);
            count2 = count2 + 1;
        end;
    end;
    if (count1 > length(t1))
        X(i+1:end,:) = X2(count2:end,:);
        Y(i+1:end,:) = Y2(count2:end,:);
        d(i+1:end) = 0;
        t(i+1:end) = t2(count2:end);
    else
        X(i+1:end,:) = X1(count1:end,:);
        Y(i+1:end,:) = Y1(count1:end,:);
        d(i+1:end) = 1;
        t(i+1:end) = t1(count1:end);
    end;
else
    X = X1;
    Y = Y1;
    t = t1;
end;
%indices of unique failure times in ft_ix (last index when ties happen)
st_ix = find(d==1);
[ft,ft_ix] = unique(t(st_ix),'last');
ft_ix = st_ix(ft_ix);
nft = length(ft);
%handling ties in failure times by substracting a very small random number
rand('state',sum(100*clock));
for j=1:nft
    i = 1;
    while (ft_ix(j)-i>0) && (ft(j)==t(ft_ix(j)-i))
        i = i + 1;
    end;
    nties = i-1;
    tt = t(ft_ix(j)-nties:ft_ix(j)) - 1e-5*(1 + rand(nties+1,1));
    tX = X(ft_ix(j)-nties:ft_ix(j),:);
    tY = Y(ft_ix(j)-nties:ft_ix(j),:);
    [stt,stt_ix] = sort(tt);
    t(ft_ix(j)-nties:ft_ix(j)) = stt;
    X(ft_ix(j)-nties:ft_ix(j),:) = tX(stt_ix,:);
    Y(ft_ix(j)-nties:ft_ix(j),:) = tY(stt_ix,:);
end;
nft = length(st_ix);
p = p+1;
pval = ones(p,nv);
rho = zeros(p,nv);
r = zeros(nft,p);
display('Checking the proportional hazards assumption at each location ...');
for j=1:nv
    if ~isempty(stats_PH(j).Bhat)
        if length(stats_PH(j).Bhat)==p
            XD =  [X Y(:,j)];
            for i=1:nft
                lprv = exp(XD(st_ix(i):end,:)*stats_PH(j).Bhat);
                r(i,:) = XD(st_ix(i),:) - (lprv'*XD(st_ix(i):end,:))./sum(lprv);
            end;
            [rho(:,j),pval(:,j)] = corr(r,[1:nft]');
        else
            error([' The number of elements of stats(' num2str(j) ').Bhat'...
                ' is different from ' num2str(p)]);
        end;
    end;
end


%kkinverse 'Inverse Filtering (Restoration) in Fourier Frequency Domain'
% This MatLab function was automatically generated by a converter (KhorosToMatLab) from the Khoros kinverse.pane file
%
% Parameters:
% InputFile: i1 'Source data object (FFT)', required: 'source data object (FFT)'
% InputFile: i2 ' PSF data object (FFT)', required: 'PSF data object (FFT)'
% OutputFile: o 'Filtered data object (FFT)', required: 'filtered data object (FFT)'
%
% Example: o = kkinverse({i1, i2}, {'i1','';'i2','';'o',''})
%
% Khoros helpfile follows below:
%
%  PROGRAM
% kinverse - Inverse Filtering (Restoration) in Fourier Frequency Domain
%
%  DESCRIPTION
% .I kinverse
% performs restoration of data in the spatial domain by inverse filtering in
% the frequency domain.  The idea is to deconvolve the blurring function
% from the blurred data.  A few problems exist in performing this operation,
% as described in great detail in any good image processing book.
%
% The major difficulty lies in how small the denominator is allowed to be
% before you call it garbage.  The value chosen is supplied as the
% -t thresh value.  Ideally, this will be just above the noise floor in the
% image, but experimentation is the best guide here.
% .I kinverse
% places zeros in the output data for those data points where the PSF data object
% magnitude is below the threshold. In this respect, kinverse is actually an
% implementation of a "pseudoinverse\fR filter.
%
% Note that the input data objects will be treated simply as a field of complex
% data, presumably obtained by application of an FFT, although the data could
% be generated by many other means. The output data will be either KCOMPLEX if
% the input data is any type other than KDCOMPLEX. Otherwise, the output will be
% of type KDCOMPLEX. Data objects with maps attached are not supported.
%
%
%
%  EXAMPLES
%
%  "SEE ALSO"
%
%  RESTRICTIONS
%
%  REFERENCES
% R.C. Gonzalez, P. Wintz, "Digital Image Processing", Addison-Wesley 1987,
% ISBN 0-201-11026-1, pp. 221-224.
%
%  COPYRIGHT
% Copyright (C) 1993 - 1997, Khoral Research, Inc. ("KRI")  All rights reserved.
%

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

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


% The function used with the program: Quadrotor control

function xdot = quad_control_fn(t,x)

global Jtp Ixx Iyy Izz b d l m g Kpz Kdz Kpp Kdp Kpt Kdt Kpps Kdps ZdF PhidF ThetadF PsidF ztime phitime thetatime psitime Zinit Phiinit Thetainit Psiinit Uone Utwo Uthree Ufour Ez Ep Et Eps

% The desired values
% Changes in Z start at t = 3, changes in Phi start at t = 1, changes in
% Theta start at the origin, and chanfes in Psi start at t= 2
% If you want that all start at the origin simply remove the conditions

% time for change start of each variable
ztime = 3;
phitime = 1;
thetatime = 0.2;
psitime = 2;
%%% HEIGHT %%%
if t < ztime
 Zd = Zinit;
end

if t >= ztime
 Zd = ZdF;
end
%%%%%%%%%%%%%%
%%% Phi %%%
if t < phitime
 Phid = Phiinit;
end

if t >= phitime
 Phid = PhidF;
end
%%%%%%%%%%%%%%
%%% Theta %%%
if t < thetatime
 Thetad = Thetainit;
end

if t >= thetatime
 Thetad = ThetadF;
end
%%%%%%%%%%%%%%
%%% Psi %%%
if t < psitime
 Psid = Psiinit;
end

if t >= psitime
 Psid = PsidF;
end
%%%%%%%%%%%%%%

PsidF = 2*pi;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

thetaddot = 0;
phiddot = 0;
psiddot = 0;
Zddot = 0;
% Zd = 10;

    % Bounding the angles within the -2*pi / 2*pi range
    if (x(7)> 2*pi || x(7)< - 2*pi)
        x(7) = rem(x(7),2*pi);
    end

    if (x(9)> 2*pi || x(9)< - 2*pi)
        x(9) = rem(x(9),2*pi);
    end

    if (x(11)> 2*pi || x(11)< - 2*pi)
        x(11) = rem(x(11),2*pi);
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%PD-Z-Control%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Evaluate the Controls
    U = [];    % The control vector
    U(1) = m*(g + Kpz*(Zd - x(5)) + Kdz*( - x(6)))/(cos(x(9))*cos(x(7)));   % Total Thrust on the body along z-axis
    U(2) = (Kpp*(Phid - x(7)) + Kdp*( - x(8)));   % Roll input
	U(3) = (Kpt*(Thetad - x(9)) + Kdt*( - x(10)));   % Pitch input
	U(4) = (Kpps*(Psid - x(11)) + Kdps*( - x(12)));   % Yawing moment

    U = real(U);
    U = [U(1);U(2);U(3);U(4)];    % The control vector

%     % Bounding the controls
%     if U(1) > 15.7
%         U(1) = 15.7;
%     end
%
%     if U(1) < 0
%         U(1) = 0;
%     end
%
%     for j = 2:4
%         if U(j) > 1
%         U(j) = 1;
%         end
%
%         if U(j) < -1
%         U(j) = -1;
%         end
%     end

    % Calculation of angular velocities
    omegasqr(1) = (1/4*b)*U(1) + (1/2*b*l)*U(3) - (1/4*d)*U(4);
    omegasqr(2) = (1/4*b)*U(1) - (1/2*b*l)*U(2) + (1/4*d)*U(4);
    omegasqr(3) = (1/4*b)*U(1) - (1/2*b*l)*U(3) - (1/4*d)*U(4);
    omegasqr(4) = (1/4*b)*U(1) + (1/2*b*l)*U(2) + (1/4*d)*U(4);
    omegasqr = real(omegasqr);

    omega(1) = sqrt(omegasqr(1));
    omega(2) = sqrt(omegasqr(2));
    omega(3) = sqrt(omegasqr(3));
    omega(4) = sqrt(omegasqr(4));
        % Bounding the angular velocities
    for j = 1:4
        if omega(j) > 523
            omega(j) = 523;
        end

        if omega(j) < 125
            omega(j) = 125;
        end
    end
    omegasqr(1) = (omegasqr(1))^2;
    omegasqr(2) = (omegasqr(2))^2;
    omegasqr(3) = (omegasqr(3))^2;
    omegasqr(4) = (omegasqr(4))^2;
%     % Bounding the angular velocities
%     for j = 1:4
%         if omegasqr(j) > 523
%             omegasqr(j) = 523;
%         end
%
%         if omegasqr(j) < 125
%             omegasqr(j) = 125;
%         end
%     end
    % Disturbance
    Omega = d*(- sqrt(omegasqr(1)) + sqrt(omegasqr(2)) - sqrt(omegasqr(3)) + sqrt(omegasqr(4)));

% Evaluation of the State space wrt H-frame
    xdot(1) = x(2); % Xdot
    xdot(2) = (sin(x(11))*sin(x(7)) + cos(x(11))*sin(x(9))*cos(x(7)))*(U(1)/m);    % Xdotdot
    xdot(3) = x(4); % Ydot
    xdot(4) = (-cos(x(11))*sin(x(7)) + sin(x(11))*sin(x(9))*cos(x(7)))*(U(1)/m);	% Ydotdot
    xdot(5) = x(6); % Zdot
    xdot(6) = - g + (cos(x(9))*cos(x(7)))*(U(1)/m);    % Zdotdot
    xdot(7) = x(8); % phydot
    xdot(8) = ((Iyy - Izz)/Ixx)*x(10)*x(12) - (Jtp/Ixx)*x(10)*Omega + (U(2)/Ixx); % pdot = phydotdot
    xdot(9) = x(10);    % thetadot
    xdot(10) = ((Izz - Ixx)/Iyy)*x(8)*x(12) + (Jtp/Iyy)*x(8)*Omega + (U(3)/Iyy);	% qdot = thetadotdot
    xdot(11) = x(12);   % thetadot
    xdot(12) = ((Ixx - Iyy)/Izz)*x(8)*x(10) + (U(4)/Izz);	% rdot = psidotdot

xdot = xdot';


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
%   - data                 Hand, Omega=(0,20)x(0,25),
%   - viewer               viewImage2D
%   - interpolation        splineInter
%   - distance             SSD
%   - regularizer          mfCurvature
%   - optimizer            Gauss-Newton
% ===============================================================================

clear; close all; help(mfilename)

setup2DhandData
imgModel('reset','imgModel','splineInter','regularizer','moments','theta',0.01);
distance('reset','distance','SSD');
trafo('reset','trafo','affine2D');
regularizer('reset','regularizer','mfCurvature','alpha',1000);

yc = MLIR(ML,'parametric',1);
showResults(ML,yc);

%==============================================================================


function featvec = MRS4fast(im)

[featvec] = MR8fast(im);
featvec = featvec';

featvec = cat(2, featvec(:, 1:2), max(featvec(:, 3:5), [], 2), max(featvec(:, 6:8), [], 2));


function doinvers_michael(mycat)
    % doinvers_michael calculates orientation of the stress tensor based on Gephard's algorithm.
    % stress tensor orientation. The actual calculation is done using a call to a fortran program.
    %
    % Stefan Wiemer 03/96

    % todo: HOW DOES THIS DIFFER FRON DOINVERSE?

    global mif1 mif2
    global tmpi cumu2
    % Save path to current folder
    sPath = pwd;
    ZG = ZmapGlobal.Data;
    fs = filesep;

    report_this_filefun();

    % Number of bootstraps
    rep = 2000;
    %if isunix ~= 1
    % errordlg('Misfit calculation only implemented for UNIX version! ');
    % return
    %end

    % making assumption that columns 10-12 were dip, dip idirection, etc.
    if isempty(mycat.Dip) || isempty(mycat.DipDirection) || isempty(mycat.Rake)
        errordlg('You do not have the required fields to calculate misfit');
        return
    end

    hodis = fullfile(hodi, 'external');

    tmpi = [ZG.mycat.Dip , ZG.mycat.DipDirection , ZG.mycat.Rake];

    cd(hodis);

    fid = fopen('data2','w');
    str = ['Inversion data'];
    str = str';

    fprintf(fid,'%s  \n',str');
    fprintf(fid,'%7.3f  %7.3f  %7.3f \n',tmpi');

    fclose(fid);

    delete data2.slboot Xtemp.slboot
    disp('Now doing inversion ... ')
    watchon
    helpdlg('The inversion is running right now ... it will take a few seconds ... please wait until results appear ');

    % slfast calculates the best solution for the stress tensor according to
    % Michael(1987): creates data2.slboot for bootslickw
    %[stat, res] = unix(['.' fs 'slfast data2 ']);

    slick_program = append_system_specific_postfix('slick');
    slfast_program = append_system_specific_postfix('slfast');
    bootslickw_program = append_system_specific_postfix('bootslickw');

    [stat, res] =  system([fullfile(ZG.hodi, slfast_program), ' data2'])

    if strcmp(res,'') == 0
        helpdlg('It seems that the inversion did not run, because the command slick_* data2 could not be executed. Is the executebale slick in the directory extrenal? ','error inverting');
        return
    end
    % slick calculates the best solution for the stress tensor according to
    % Michael(1987): creates data2.oput
    system([fullfile(ZG.hodi, slick_program), ' data2'])

    % Get data from data2.oput
    sFilename = 'data2.oput';
    [fBeta, fStdBeta, fTauFit, fAvgTau, fStdTau] = import_slickoput(sFilename);

    load data2.slboot
    % d0: Best solution for stress tensor
    d0 = data2;

    % bootslickw resamples the data and uses slfast to calculate the best fitting stress tensor
    system(sprintf('"%s" data2 %d 0.5', fullfile('.',bootslickw_program), rep));

    disp(' Done !  ')
    watchoff

    load Xtemp.slboot
    l = 2:2:2*rep;
    % d1: Bootstrap stress tensor solutions
    d1 = Xtemp(l,:);

    % Back to the original folder
    cd(sPath);

    % Now plot the results
    figure
    wulff
    set(gca,'NextPlot','add')

    %gridsize
    radius=0.05;

    % Plotting the bootstrap solutions of S1
    X = [d1(:,3) d1(:,2) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin

    colormap(hot);
    brighten(0.5);
    densiter=denserfocalv2(rho,theta,radius);
    [xvec, yvec] =meshgrid(-1:0.01:1,-1:0.01:1);
    zvec=griddata(densiter(:,1),densiter(:,2),densiter(:,3),xvec,yvec);
    p11=contourf(xvec,yvec,zvec);

    %calculate 95% confidence interval
    bpoints(:,1)= rho .* cos(theta); %xval bootstrap
    bpoints(:,2)= rho .* sin(theta); %yval bootstrap

    X = [d0(2,3) d0(2,2) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin
    solpoint(:,1)= rho .* cos(theta); %xval solution
    solpoint(:,2)= rho .* sin(theta); %yval solution

    cline = contour_dense(bpoints, solpoint, 0.95, 36);
    plot(cline(:,1),cline(:,2),'k', 'LineWidth',3,'LineStyle','--',...
        'DisplayName','95%');

    % Plotting the bootstrap solutions of S2
    X = [d1(:,5) d1(:,4) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin

    densiter=denserfocalv2(rho,theta,radius);
    [xvec, yvec] =meshgrid(-1:0.01:1,-1:0.01:1);
    zvec=griddata(densiter(:,1),densiter(:,2),densiter(:,3),xvec,yvec);
    p12=contourf(xvec,yvec,zvec);

    %calculate 95% confidence interval
    bpoints(:,1)= rho .* cos(theta); %xval bootstrap
    bpoints(:,2)= rho .* sin(theta); %yval bootstrap

    X = [d0(2,5) d0(2,4) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin
    solpoint(:,1)= rho .* cos(theta); %xval solution
    solpoint(:,2)= rho .* sin(theta); %yval solution

    cline = contour_dense(bpoints, solpoint, 0.95, 36);
    plot(cline(:,1),cline(:,2),'k', 'LineWidth',3,'LineStyle','--',...
        'DisplayName','95%');

    % Plotting the bootstrap solutions of S3
    X = [d1(:,7) d1(:,6) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin

    densiter=denserfocalv2(rho,theta,radius);
    [xvec, yvec] =meshgrid(-1:0.01:1,-1:0.01:1);
    zvec=griddata(densiter(:,1),densiter(:,2),densiter(:,3),xvec,yvec);
    p13=contourf(xvec,yvec,zvec);

    %calculate 95% confidence interval
    bpoints(:,1)= rho .* cos(theta); %xval bootstrap
    bpoints(:,2)= rho .* sin(theta); %yval bootstrap

    X = [d0(2,7) d0(2,6) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin
    solpoint(:,1)= rho .* cos(theta); %xval solution
    solpoint(:,2)= rho .* sin(theta); %yval solution

    cline = contour_dense(bpoints, solpoint, 0.95, 36);
    plot(cline(:,1),cline(:,2),'k', 'LineWidth',3,'LineStyle','--',...
        'DisplayName','95%');

    % Plot the best solution
    % Replaced d1 with d0, JW
    X = [d0(2,3) d0(2,2) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin
    xp = rho .* cos(theta);
    yp = rho .* sin(theta);
    pl1a = plot(xp,yp,'ks'); %s
    set(pl1a,'LineWidth',2,'MarkerSize',12,'Markerfacecolor','w')
    set(gca,'NextPlot','add')

    X = [d0(2,5) d0(2,4) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin
    xp = rho .* cos(theta);
    yp = rho .* sin(theta);
    pl2a = plot(xp,yp,'k^'); %^
    set(pl2a,'LineWidth',2,'MarkerSize',12,'Markerfacecolor','w')

    X = [d0(2,7) d0(2,6) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin
    xp = rho .* cos(theta);
    yp = rho .* sin(theta);
    pl3a = plot(xp,yp,'ok'); %o
    set(pl3a,'LineWidth',2,'MarkerSize',12,'Markerfacecolor','w')
    set(gcf,'color','w');

    %doing the legend
    le = legend([pl1a pl2a pl3a],'S1','S2','S3');
    set(le,'pos',[0.1 0.8 0.15 0.1]);
    set(le,'Xcolor','w','ycolor','w','box','off');

    % Compute standard deviation of phi
    fStdPhi = std(d1(:,1),1,'omitnan');

    axes('pos',[0 0 1 1 ]);
    axis off
    text(0.01,0.22,['Variance: ' num2str(d0(1,1),2) ]);
    text(0.01,0.18,['Phi: ' num2str(d0(2,1),2) ' \pm ' num2str(fStdPhi)]);
    text(0.01,0.14,['S1: trend: ' num2str(d0(2,2),4) '; plunge: '  num2str(d0(2,3),4) ]);
    text(0.01,0.1,['S2: trend: ' num2str(d0(2,4),4) '; plunge: '  num2str(d0(2,5),4) ]);
    text(0.01,0.06,['S3: trend: ' num2str(d0(2,6),4) '; plunge: '  num2str(d0(2,7),4) ]);
    text(0.78,0.22,['Tau: ' num2str(fAvgTau,4) ' \pm ' num2str(fStdTau,4)]);
    text(0.78,0.18,['Ratio: ' num2str(fTauFit,4)]);
    text(0.78,0.14,['Beta: ' num2str(fBeta,4) ' \pm ' num2str(fStdBeta,4)]);

    % Determine the faulting style based on Zoback, 1992
    ste = [d0(2,3) d0(2,2)+180 d0(2,5) d0(2,4)+180 d0(2,7) d0(2,6)+180];

    faulting_type = faultType(ste(1),ste(3),ste(5));
    if strcmpi(faulting_type,'Thrust to Strike Slip')
        l4 = pl; %?
    end

    text(0.01,0.02,['Faulting style: ' faulting_type]);
    % Link to World stress map
    uicontrol('Units','normal',...
        'Position',[.4 .0 .1 .04],'String','Info ',...
        'callback',@callbackfun_001);

    % Plot CDF and histogram of phi-values
    figure_w_normalized_uicontrolunits('Name','Phi - ratio of relative stress magnitude','Position',[700   700   472   232])
    % CDF
    subplot(1,2,1)
    vDistribution = d1(:,1);
    % Remove NaN-values
    vSel = ~isnan(vDistribution);
    vPlotDist = vDistribution(vSel,:);
    % Plot the cumulative density function
    nLen = length(vPlotDist);
    vIndices = [1:nLen]/nLen;
    vDist = sort(vPlotDist);
    plot(vDist, vIndices,'Linewidth',1.5,'Color',[0 0 0]);
    set(gca,'NextPlot','add')
    xlabel('\phi','FontSize',12,'Fontweight','bold')
    ylabel('Fraction of cases','FontSize',12,'Fontweight','bold')
    set(gca,'Box','on','Linewidth',1.5,'Xlim',[0 1])
    plot([d0(2,1) d0(2,1)],[0 1],'Color',[0.8 0 0],'Linewidth',1.5)
    legend(' \phi CDF bootstrap','\phi org. data')
    % Histogram
    subplot(1,2,2)
    histogram(vDistribution,0:0.05:1)
    set(gca,'Xlim',[0 1]);
    xlabel('\phi','FontSize',12,'Fontweight','bold')
    ylabel('Frequency','FontSize',12,'Fontweight','bold')

    function callbackfun_001(mysrc,myevt)

        callback_tracker(mysrc,myevt,mfilename('fullpath'));
        web http://www.world-stress-map.org/;
    end

    function ftype = faultType(s1_east, s2_west, s3_north)
        % faultTypes determine faulting ftype from the s1_east,s2_west,s3_north plunge values
        % returns a string description
        ftype = 'Unknow';

        if s1_east>52                         && s3_north<35
            ftype = 'Normal';
        end

        if 40<s1_east  && s1_east<52               && s3_north<20
            ftype = 'Normal to Strike Slip';
        end

        if s1_east<40            && s2_west>45     && s3_north<20
            ftype = 'Strike Slip';
        end

        if s1_east<20            && s2_west>45     && s3_north<40
            ftype = 'Strike Slip';
        end

        if s1_east<20                         && 40<s3_north && s3_north<52
            ftype = 'Thrust to Strike Slip';
        end

        if s1_east<35                         && s3_north>52
            ftype = 'Thrust';
        end
    end

end


function depth =denoise(depth,camera_D)

XYZcamera(:,:,1)=camera_D.X .* depth;
XYZcamera(:,:,2)=camera_D.Y .* depth;
XYZcamera(:,:,3)=depth .* (~isnan(camera_D.X) & ~isnan(camera_D.Y));

%figure;imagesc(XYZcamera(:,:,3)); axis equal; axis tight;

X = reshape(XYZcamera,[],3);

validID = find(X(:,3)~=0);

X = X(validID,:);

%points2ply('original.ply', X);

% denoise

kdOBJ = KDTreeSearcher(X);

[~,mindist] = knnsearch(kdOBJ,X,'K',4);

threshold1 = 0.1;
threshold2 = 0.2;
threshold3 = 0.25;
threaholdFurthest = 8; %15;
threaholdClosest = 0.5;

goodones = mindist(:,2)<threshold1 & mindist(:,3)<threshold2 & mindist(:,4)<threshold3 & X(:,3)<threaholdFurthest & X(:,3)>threaholdClosest ;

XYZcamera( 2*camera_D.width*camera_D.height + validID(~goodones)) = 0;
X = X(goodones,:);

%figure;imagesc(XYZcamera(:,:,3)); axis equal; axis tight;

%points2ply('filter1.ply', X);

depth3Max = ordfilt2(XYZcamera(:,:,3),23,true(5));

outliers = XYZcamera(:,:,3) > (depth3Max + 0.5) & (depth3Max>0);
XYZcamera( 2*camera_D.width*camera_D.height + find(outliers)) = 0;

%figure;imagesc(XYZcamera(:,:,3)); axis equal; axis tight;

%depthMed = medfilt2(XYZcamera(:,:,3), [5 5]);
%XYZcamera(:,:,3)

%X = reshape(XYZcamera,[],3);
%validID = find(X(:,3)~=0);
%X = X(validID,:);
%points2ply('filter2.ply', X);

depth = XYZcamera(:,:,3);


function [Res] = rmsr_inv(P, VecI, Mat, Pth)

% rmsr_inv replaces all descriptors from Mat (except the ones in the given descriptor vector) in the given position Pth and returns
%      the vector with higher S_err, using multiple linear regression analysis.
%
%
%	   Input:
%             P             Property vector
%             VecI          Descriptor vector
%             Mat           Descriptor matrix
%             Pth           Path to follow
%
%     Returns:
%
%             Res           Vector containing in the first place the
%                           S_err, and afterwards the corresponding descriptor vector
%
%
%
% Andrew G. Mercader
% INIFTA, La Plata, Argentina
% Created: 30 Jan 2007

if (nargin < 4)
   error('rmsr_inv requires at least 4 input variables. Type ''help rmsr_inv''.');
end

[k, n_m] = size(Mat);

Num=[1:n_m];
Vec2=VecI;
Vec2(Pth)=[];
Num(Vec2)=[];

[k,n_n]=size(Num);

Smax=0.00000000000000000000000000000000001;   % A very big number compared to a normal S necessary just to start the program
for j=1:n_n;
VecI(Pth)=Num(j);
Ser=rms_inv(P,VecI,Mat);
    if (Ser>Smax)
         Smax=Ser;
        desc=Num(j);
    end
end
VecI(Pth)=desc;
Res=[Smax, VecI];
%End of rmsr_inv


% Steering A Linear Array Example
%
% This example demonstrates how to use k-Wave to steer a tone burst from a
% linear array transducer in 2D. It builds on the Simulating Transducer
% Field Patterns Example.
%
% author: Bradley Treeby
% date: 10th December 2009
% last update: 24th August 2014
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave 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.
%
% k-Wave 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 Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

clear all;

% =========================================================================
% SIMULATION
% =========================================================================

% create the computational grid
Nx = 128;           % number of grid points in the x (row) direction
Ny = Nx;            % number of grid points in the y (column) direction
dx = 50e-3/Nx;    	% grid point spacing in the x direction [m]
dy = dx;            % grid point spacing in the y direction [m]
kgrid = makeGrid(Nx, dx, Ny, dy);

% define the properties of the propagation medium
medium.sound_speed = 1500;  % [m/s]
medium.alpha_power = 1.5;   % [dB/(MHz^y cm)]
medium.alpha_coeff = 0.75;  % [dB/(MHz^y cm)]

% create the time array
[kgrid.t_array, dt] = makeTime(kgrid, medium.sound_speed);
kgrid.t_array = 0:dt:500*dt;

% define source mask for a linear transducer with an odd number of elements
num_elements = 21;      % [grid points]
x_offset = 25;          % [grid points]
source.p_mask = zeros(Nx, Ny);
start_index = Ny/2 - round(num_elements/2) + 1;
source.p_mask(x_offset, start_index:start_index + num_elements - 1) = 1;

% define the properties of the tone burst used to drive the transducer
sampling_freq = 1/dt;   % [Hz]
steering_angle = 30;    % [deg]
element_spacing = dx;   % [m]
tone_burst_freq = 1e6;  % [Hz]
tone_burst_cycles = 8;

% create an element index relative to the centre element of the transducer
element_index = -(num_elements - 1)/2:(num_elements - 1)/2;

% use geometric beam forming to calculate the tone burst offsets for each
% transducer element based on the element index
tone_burst_offset = 40 + element_spacing*element_index*sin(steering_angle*pi/180)/(medium.sound_speed*dt);

% create the tone burst signals
source.p = toneBurst(sampling_freq, tone_burst_freq, tone_burst_cycles, 'SignalOffset', tone_burst_offset);

% assign the input options
input_args = {'DisplayMask', source.p_mask};

% run the simulation
kspaceFirstOrder2D(kgrid, medium, source, [], input_args{:});

% =========================================================================
% VISUALISATION
% =========================================================================

% plot the input time series
figure;
num_source_time_points = length(source.p(1,:));
[t_sc, scale, prefix] = scaleSI(kgrid.t_array(num_source_time_points));
stackedPlot(kgrid.t_array(1:num_source_time_points)*scale, source.p);
xlabel(['Time [' prefix 's]']);
ylabel('Input Signals');


% Sample of Linear Blend Skinning in Matlab
% This code is non optimized nor necessarily correct.
% Also, it has only been tested on the data in this folder so I dont know
% how it will behave on other data.

% Aaron Wetzler, 2013

%% -------------------------------------------------------------------------
clf
close all
clear all
clc

% Load the faces, vertices and weights
faces = csvread('handfaces.txt');
V0 = csvread('handverts.txt');
V0=V0';
N = size(V0,1);
weights = csvread('handweights.txt');
sk = loadbvh('HandBase.bvh');

% Mapping to correct the ordering of the weights to the bones for the
% HandBase.bvh file
wmap = [1 2 3 4 0 15 16 17 0 5 6 7 0 8 9 10 0 11 12 13 0 14 0];
wzero = wmap==0;

% This will likely crash a machine if there are two many bones and too many
% animation frames
W = sparse(weights(:,1)+1,weights(:,2)+1,weights(:,3));
W = full(W);%W = W./repmat(sum(W,2),[1 size(W,2)]);

W = W(:,wmap+wzero);
W(:,wzero)=0;

% Currently there are some rows in W which have up to 6 nonzero weights.
% The industry accepted number is 4 so should probably zero the smaller
% ones.

% -------------------------------------------------------------------------

%Display the mesh

cla
for displayWeightIndex=find(~wzero)
    h = trisurf(faces+1,V0(1,:),V0(2,:),V0(3,:),W(:,displayWeightIndex));
    axis equal; axis vis3d;
    shading interp;
    set(h,'FaceAlpha',0.5)
    axis([-200 200 -100 100 -100 500]);
    pause(0.2);
end
hold on

%% -------------------------------------------------------------------------
% Display the skinned armature animation

for j=2:258

    V=V0*0;

    cla
    for i=1:length(sk)
        % -------------------------------------------------------------------------
        % Display the bones
        if sk(i).parent
            s=sk(i).t_xyz(:,j); sp = sk(sk(i).parent).t_xyz(:,j);
            plot3([s(1) sp(1)],[s(2) sp(2)],[s(3) sp(3)],'r','LineWidth',5);
        end
        plot3(sk(i).t_xyz(1,j),sk(i).t_xyz(2,j),sk(i).t_xyz(3,j),'g.','MarkerSize',10);
        if sk(i).Nchannels
            plotax(sk(i).t_xyz(:,j)',sk(i).T(1:3,1:3,j)*sk(i).R0,15)
        end
        % -------------------------------------------------------------------------

        if sk(i).Nchannels
            % Takes a local bone coordinate and puts it into the new pose global position
            poseMatrix(:,:,i,j) = [(sk(i).T(1:3,1:3,j)*sk(i).R0) sk(i).t_xyz(:,j);0 0 0 1];
            % Takes a local bone coordinate and puts it into the base pose global position
            restMatrix(:,:,i) = [((sk(i).R0)) sk(i).head0 ;0 0 0 1];

            % The matrix T for a bone takes a point from global bind space to its new
            % position also in global space as dictated by that bone

            % -------------------------------------------------------------------------------
            % To go from base pose to new pose position we
            % undo the restmatrix and then apply the posematrix i.e.
            % if we have a vertex v from the bind pose then to see where
            % it would be sent by bone i for pose j we would apply
            % vj = M(:,:,j)*[v;1]

            M(:,:,j) = poseMatrix(:,:,i,j)*inv(restMatrix(:,:,i));
            R = M(1:3,1:3,j);
            t = M(1:3,4,j);

            % We add the influence from every bone
            V = V + (R*V0 + repmat(t,1,length(V0))) * spdiags(W(:,i),0,length(V),length(V));

            % --------------------------------------------------------------------------------

        end
    end

    % -------------------------------------------------------------------------
    % Display nicely
    h = trisurf(faces+1,V(1,:),V(2,:),V(3,:),W(:,2)*0);
    axis off;
    axis equal
    shading interp;
    light
    lighting phong
    set(h,'FaceAlpha',0.8)
    drawnow

end
%%

% Code to extract specific joint angles
% Bone i, animation frame j
i = 20; j=2;
T = poseMatrix(1:3,1:3,i,j); Tp = poseMatrix(1:3,1:3,i-1,j); T_0 = restMatrix(1:3,1:3,i); Tp_0 = restMatrix(1:3,1:3,i-1);
R = (T_0')*(Tp_0)*(Tp')*(T);
[Rx Ry Rz] = dcm2angle(R,'XYZ');
Rx = rad2deg(-Rx); Ry = rad2deg(-Ry); Rz = rad2deg(-Rz);

[Rx Ry Rz]


function marginal = marginal_family(engine, i, t)
% MARGINAL_FAMILY Compute the marginal on the specified family (ff)
% marginal = marginal_family(engine, i, t)

if engine.filter
  error('can''t currently use marginal_family when filtering with ff');
end

if nargin < 3, t = 1; end

% The method is similar to the following HMM equation:
% xi(i,j,t) = normalise( alpha(i,t) * transmat(i,j) * obsmat(j,t+1) * beta(j,t+1) )
% where xi(i,j,t) = Pr(Q(t)=i, Q(t+1)=j | y(1:T))

bnet = bnet_from_engine(engine);
ss = length(bnet.intra);

if myismember(i, engine.onodes)
  ps = parents(bnet.dag, i);
  p = ps(1);
  marginal = pot_to_marginal(engine.marginals{ps(1),t});
  fam = ([ps i]) + (t-1)*ss;
elseif t==1
  marginal = pot_to_marginal(engine.marginals{i,t});
  fam = i + (t-1)*ss;
else
  pot = engine.CPDpot{i,t};
  c = engine.obschild(i);
  if c>0
    pot = multiply_by_pot(pot, engine.CPDpot{c,t});
  end
  pot = multiply_by_pot(pot, engine.back{i,t});
  ps = parents(bnet.dag, i+ss);
  for p=ps(:)'
    pot = multiply_by_pot(pot, engine.fwd{p,t-1});
  end
  marginal = pot_to_marginal(normalize_pot(pot));
  fam = ([ps i+ss]) + (t-2)*ss;
end

% we convert the domain to the unrolled numbering system
% so that update_ess extracts the right evidence.
marginal.domain = fam;


function [w, momentum] = sgd(w, momentum, grad, opts, lr)
%SGD
%   Example SGD solver, with momentum, for use with CNN_TRAIN and
%   CNN_TRAIN_DAG.
%
%   The convergence of SGD depends heavily on the learning rate (set in the
%   options for CNN_TRAIN and CNN_TRAIN_DAG).
%
%   If called without any input argument, returns the default options
%   structure.
%
%   Solver options: (opts.train.solverOpts)
%
%   `momentum`:: 0.9
%      Parameter for Momentum SGD; set to 0 for standard SGD.
%
%   Note: for backwards compatibility, the parameter can also be set in
%   opts.train.momentum.

% Copyright (C) 2016 Joao F. Henriques.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

if nargin == 0 % Return the default solver options
  w = struct('momentum', 0.9);
  return;
end
if isempty(momentum)
  momentum = 0 ;
end

momentum = opts.momentum * momentum - grad ;
w = w + lr * momentum ;


function [ Y ] = classify2( Model, X )

    % Extract Features from the test data
    data = getImages(X);

    weights1 = Model.weights1;
    weights2 = Model.weights2;
    projection = Model.projection;

    % Project the test data on the new dimension
    xTest = getFeatures(data, projection);

    % Classify the images
    [Y, ~] = NNClassify(weights1, weights2, xTest);
end

function [Features] =  getImages(data)
    Features = [];
    for i = 1:size(data,1)
        image = reshape(data(i,:),[32,32,3]);
        image = imresize(image,4);
        feat = extract_feature(image);
        Features = horzcat(Features,feat);
    end
    Features = Features';
end

function [Feats] = getFeatures(Features, projection)
    featureSize = size(Features,2);

    % Perform Mean Normalization on the Feature Matrix.
    % Each pixel can have intensity values from 0 - 255
    for i = 1 : featureSize
        Features(:,i) = Features(:,i) - mean(Features(:,i));
    end

    % Project the Features on the new dimension
    Feats = projection * Features';
    Feats = Feats';
end


function [Ain, Cin,  bin, fin, center, res] = greedyROI_corr(Y, K, options, sn, debug_on, save_avi)
%% a greedy method for detecting ROIs and initializing CNMF. in each iteration,
% it searches the one with large (peak-median)/noise level and large local
% correlation
%% Input:
%   Y:  d X T matrx, imaging data
%   K:  scalar, maximum number of neurons to be detected.
%   options: struct data of paramters/options
%       d1:     number of rows
%       d2:     number of columns
%       gSiz:   maximum size of a neuron
%       nb:     number of background
%       min_corr: minimum threshold of correlation for segementing neurons
%   sn:     d X 1 vector, noise level of each pixel
%   debug_on: options for showing procedure of detecting neurons
%% Output:
%       Ain:  d X K' matrix, estimated spatial component
%       Cin:  K'X T matrix, estimated temporal component
%       bin:  d X nb matrix/vector, spatial components of the background
%       Cin:  nb X T matrix/vector, temporal components of the background
%       center: K' X 2, coordinate of each neuron's center
%       res:  d X T, residual after initializing Ain, Cin, bin, fin

%% Author: Pengcheng Zhou, Carnegie Mellon University.
% the method is an modification of greedyROI method used in Neuron paper of Eftychios
% Pnevmatikakis et.al. https://github.com/epnev/ca_source_extraction/blob/master/utilities/greedyROI2d.m
%% In each iteration of initializing neurons, it searchs the one with maximum
% value of (max-median)/noise * Cn, which selects pixels with high SNR and
% local correlation.

%% parameters
if exist('sn', 'var')&& ~(isempty(sn))
    Y_std = sn;
else
    Y_std = std(Y, 0, ndims(Y));
end
Y_std = Y_std(:);
if ~exist('debug_on', 'var'); debug_on = false; end
if ~exist('save_avi', 'var'); save_avi=false; end
d1 = options.d1;
d2 = options.d2;
gSig = options.gSig;
gSiz = options.gSiz;
if and(isempty(gSiz), isempty(gSig)); gSig = 3; gSiz = 10; end
if isempty(gSiz); gSiz=3*gSig; end
if isempty(gSig); gSig=gSiz/3; end
min_corr = options.min_corr;    % minimum local correaltion value to start one neuron
nb = options.nb;        % number of the background
pSiz = 1;       % after selecting one pixel, take the mean of square box
%near the pixel as temporal activity. the box size is (2*pSiz+1)
psf = ones(gSig)/(gSig^2);

min_snr = 3;        % minimum value of (peak-median)/sig
maxIter = 5;            % iterations for refining results
sz = 4;            %distance of neighbouring pixels for computing local correlation

if ~ismatrix(Y); Y = reshape(Y, d1*d2, []); end;
[~, T] = size(Y);       % number of frames
Ain = zeros(d1*d2, K);  % spatial components
Cin = zeros(K, T);      % temporal components
center = zeros(K, 2);   % center of the initialized components

%% compute correlation image and (max-median)/std ratio
ind_frame = round(linspace(1, T, min(T, 1000)));    % select few frames for speed issues
%tmp_noise = randn(d1*d2, length(ind_frame));
C1 = correlation_image(full(Y(:, ind_frame)), sz, d1, d2);
Cb =  zeros(size(C1)); %correlation_image(full(Y(:, ind_frame(1:3:end)))+tmp_noise(:, 1:3:end), [gSiz, gSiz+1], d1, d2);  %backgroung correlatin
Cn = C1-Cb; %here Cb is the background correlation. for 2photon imaging results. It might be useful when the background signal is large
Y_median = median(Y(:, ind_frame), 2);
Y = bsxfun(@minus, Y, Y_median);
% Y_std = sqrt(mean(Y.*Y, 2));

%% find local maximum
k = 0;      %number of found components
min_pixel = floor(gSig^2/2);  % minimum number of peaks to be a neuron
peak_ratio = full(max(Y, [], 2))./Y_std; %(max-median)/std
peak_ratio(isinf(peak_ratio)) = 0;  % avoid constant values
% save_avi = false;   %save procedures for demo
if debug_on
    figure('position', [100, 100, 800, 650]); %#ok<*UNRCH>
    subplot(331);
    imagesc(Cn, [0,1]); colorbar;
    axis equal off tight; hold on;
    title('correlation image');
    if save_avi
        avi_file = VideoWriter('greedyROI_example.avi');
        avi_file.open();
    end
end

max_thresh = min_snr * (min_corr);
while k<K
    %% find the pixel with the maximum ratio
    [max_v, ind_p] = max(peak_ratio.*(Cn(:)));
    peak_ratio(ind_p) = 0;  % no longer visit this pixel any more
    if max_v<max_thresh; break; end
    if Cn(ind_p)<min_corr; continue; end % ignore this local maximum due to small local correlation
    if  max_v/(min_corr)< min_snr;     continue;    end

    [r, c] = ind2sub([d1,d2], ind_p);

    % select its neighbours for computing correlation
    rsub = max(1, -gSiz+r):min(d1, gSiz+r);
    csub = max(1, -gSiz+c):min(d2, gSiz+c);
    [cind, rind] = meshgrid(csub, rsub);
    nr = length(rsub);  %size of the neighboring matrix
    nc = length(csub);
    ind_nhood = sub2ind([d1, d2], rind(:), cind(:));
    Y_box = Y(ind_nhood, :);

    % draw a small area near the peak and extract the mean activities
    r0 = rsub(1); c0 = csub(1);
    rsub = (max(1, -pSiz+r):min(d1, pSiz+r)) - r0+1;
    csub = (max(1, -pSiz+c):min(d2, pSiz+c)) -c0+1;
    [cind, rind] = meshgrid(csub, rsub);
    ind_peak = sub2ind([nr, nc], rind(:), cind(:));
    y0 = mean(Y_box(ind_peak, :), 1);
    y0(y0<0) = 0;

    % compute the correlation between the peak and its neighbours
    temp = reshape(corr(y0', Y_box'), nr, nc);
    active_pixel = full(temp>min_corr/2);
    l = bwlabel(active_pixel, 8);   % remove disconnected components
    active_pixel(l~=mode(l(ind_peak))) = false;
    tmp_v = sum(active_pixel(:));    %number of pixels with above-threshold correlation
    if debug_on
        subplot(332); cla;
        imagesc(reshape(peak_ratio.*Cn(:), d1, d2), [0, max_v]); colorbar;
        title(sprintf('neuron %d', k+1));
        axis equal off tight; hold on;
        plot(c,r, 'om');
        subplot(333);
        imagesc(temp, [min_corr, 1]);
        axis equal off tight;
        title('corr. with neighbours');
        subplot(3,3,4:6); cla;
        plot(y0); title('activity in the center');
        subplot(3,3,7:9); cla;
        if ~save_avi; pause; end
    end
    if tmp_v<min_pixel;         continue;  end % neuron is too small

    %% save neuron
    %   nonzero area
    data = Y_box(active_pixel(:), :);
    ind_active = ind_nhood(active_pixel(:));  %indices of active pixels within the whole frame
    peak_ratio(ind_nhood(ind_peak)) = 0;    % the small area near the peak is not able to initialize neuron anymore

    % do a rank-1 matrix factorization in this small area
    [ai, ci] = finetune2d(data, y0, maxIter);
    %     data(data<0) = 0;
    %     ai = (data*y0')/(y0*y0');
    %     ci = y0;
    if norm(ai)==0;        continue;   end
    k = k+1;
    Ain(ind_active, k) = ai;
    Cin(k, :) = ci;
    Y(ind_active, :) = data-ai*ci;
    center(k, :) = [r, c];

    if debug_on
        subplot(331);
        plot(c, r, '.r');
        subplot(332);
        plot(c,r, 'or');
        subplot(333);
        temp = zeros(nr, nc); temp(active_pixel) = ai;
        imagesc(temp);
        axis equal off tight;
        title('spatial component');
        subplot(3,3,7:9); cla;
        plot(ci); title('temporal component');
        if save_avi;
            temp = getframe(gcf);
            temp.cdata = imresize(temp.cdata, [800, 640]);
            avi_file.writeVideo(temp);
        else pause; end
    end

    if mod(k, 10)==0
        fprintf('%d/%d neurons have been detected\n', k, K);
    end

    if k==K;   break; end

    %% udpate peak_ratio and correlation image
    tmp_old = peak_ratio(ind_active);
    tmp_new = max(Y(ind_active, :), [], 2)./Y_std(ind_active);
    temp = zeros(nr, nc);
    temp(active_pixel) = max(0, tmp_old-tmp_new); % after each iteration, the peak ratio can not be increased
    peak_ratio(ind_nhood) = max(0, peak_ratio(ind_nhood) - reshape(imfilter(temp, psf), [], 1)); % update peak_ratio, results are smoothed
    Cn(ind_nhood) = correlation_image(full(Y(ind_nhood, ind_frame)), sz, nr, nc)-reshape(Cb(ind_nhood), nr, nc);  % update local correlation
end

center = center(1:k, :);
Ain = sparse(Ain(:, 1:k));
Cin = Cin(1:k, :);
Cin(Cin<0) = 0;
if save_avi; avi_file.close(); end
res = bsxfun(@plus, Y, Y_median);

%% initialize background
tsub = max(1, round(T/1000));
[bin, f] = nnmf(max(res(:, 1:tsub:T), 0), nb);
fin = imresize(f, [nb, T]);
fin = HALS_temporal(max(res, 0), bin, fin, maxIter);
bin = HALS_spatial(max(res, 0), bin, fin, [], maxIter);
end

function [ai, ci] = finetune2d(data, ci, nIter)
%do matrix factorization given the model data = ai*ci, where ai>=0
%
%Input:
%   data:   d x T matrix, small patch containing one neuron
%   ci:     initial value for trace
%   nIter  number of coordinate descent steps
%
%Output:
%   ai  M x N matrix, result of the fine-tuned neuron shape
%   ci  1 x T matrix, result of the neuron
%% copied from greedyROI.m

if ~exist('nIter', 'var'), nIter = 1; end
data(data<0)= 0;
%do block coordinate descent
for iter = 1:nIter,
    %update basis
    ai = max(0, (data*ci')/(ci*ci'));
    norm_ai = norm(ai, 2);
    if norm_ai==0; break;     end
    ai = ai/norm_ai;
    ci =  (ai'*data);
    %     ci(ci<0) = 0;
end
temp = (median(ci)-2*std(ci));
ci(ci<temp) = temp;
end


function r = mtimes(a,b,varargin)
% r = a * b

% multiplication with -1 -> inversion
if isnumeric(a)

  if size(a,2) == size(b,1)

    r = b;
    r.a = a * b.a;
    r.b = a * b.b;
    r.c = a * b.c;
    r.d = a * b.d;
    r.i = a * b.i;

    r = r.normalize;
    r.i = r.i>0.5;

  else

    assert(all(abs(a(:))==1),'Rotations can be multiplied only by 1 or -1');
    tmp = rotation.id(size(a));
    tmp.i = (1-a)./2;
    a = tmp;
  end

elseif isa(b,'vector3d')

  % apply rotation
  r = rotate_outer(b,a);

elseif isa(b,'quaternion')

  r = mtimes@quaternion(a,b,varargin{:});

else

  r = rotate_outer(b,a);

end

end


fclose all;
clear;

%I/O Files
DIRPATH='\Tight\sequential\data\';
FIMU=fopen([DIRPATH 'IMU.txt'],'r'); %%Assumed to be sampled at stricly 400Hz
FGPS=fopen([DIRPATH 'GPS.txt'],'r'); %%In order not to deal with program control stutucture it is assumed that GPS data is available at strictly 1Hz.
FCOV=fopen([DIRPATH 'Cov.txt'],'wt');
FOUT=fopen([DIRPATH 'Output.txt'],'wt');

%Load System parameters
[SenErrDef, ClkErrDef, IniErrDef, ObsErrDef, ini_pva, dt]=sys_def_v000();

%initial values
pos_e=ecef2geo_v000(ini_pva(:,1),1);
Cne=pos2Cne_v000(ini_pva(1,1),ini_pva(2,1));
Cbn=euler2dcm_v000(ini_pva(:,3));
Cbe=Cne*Cbn;
vel_e=Cne*ini_pva(:,2);

%Sensor error models
[Aimu, Bimu, Cimu, Dimu, sP0imu]=imu_modTI_v000(SenErrDef);
Qimu=Bimu*Bimu';
Rimu=Dimu*Dimu';
%Clock Error model
[Aclk, Bclk, Cclk, Dclk, sP0clk]=imu_modTI_v000(ClkErrDef);

%%In fact this part should be performed inside the propagate() routine.
%However, as I fixed the the propagation period, I simply discretize it with
%the known dt;
mx_a = 40*dt*[-Aclk,Bclk*Bclk';zeros(size(Aclk)),Aclk'];
mx_b = expm(mx_a);
STMclk = mx_b(3:4,3:4)';
Qclk = STMclk*mx_b(1:2,3:4);

%Initial covariance
nst=17;
P=zeros(nst);
P(1:3,1:3)=IniErrDef.pos_sP*IniErrDef.pos_sP';
P(4:6,4:6)=IniErrDef.vel_sP*IniErrDef.vel_sP';
P(7:9,7:9)=IniErrDef.att_sP*IniErrDef.att_sP';
P(10:15,10:15)=sP0imu*sP0imu';  %IMU errors
P(16:17,16:17)=sP0clk*sP0clk';  %Clock Errors

%Augmented states
Ximu=zeros(size(Aimu,1),1);
Xclk=zeros(size(Aclk,1),1);

pr_ctr=0;
imu_inc=zeros(6,1);
%Read the first IMU data
imu_data=fscanf(FIMU, '%f', 6);	%order=[Gyro;Acc]
while(~feof(FIMU))
    acc=imu_data(4:6)-Ximu(1:3);    %Correct them with error estimates
    gyro=imu_data(1:3)-Ximu(4:6);

    %Total IMU increments (used for low rate propagation)
    imu_inc=imu_inc+[acc;gyro]*dt;

    %INS calculations
    [Cbe, vel_e, pos_e]=strapdown_ecef_dcm_v000(Cbe, vel_e, pos_e, acc, gyro, dt);
    pr_ctr=pr_ctr+1;

    %Propagate the covariance and states at 10Hz
    if (~mod(pr_ctr,40))
        %Navigation model
        [STMNav QNav]=mdl_ecef_dcm_v000(Cbe, imu_inc(1:3)/0.1, Aimu, Qimu, Cimu, Rimu, dt*40);

        %Augment the clock model to nav model
        STM=diagmat_v000(STMclk,STMNav,1);
        Q=diagmat_v000(Qclk,QNav,1);

        %Propagate Covariance
        P=STM*P*STM'+Q;

        %Propagate IMU and Clock States
        Ximu=STMNav(10:15,10:15)*Ximu;
        Xclk=STMclk*Xclk;

        imu_inc=zeros(6,1);
    end

    %%Record the outputs
    if (~mod(pr_ctr,40))
        Llh=ecef2geo_v000(pos_e,0);
        Cne=pos2Cne_v000(Llh(1),Llh(2));
        vel_n=Cne'*vel_e;
        Cbn=Cne'*Cbe;
        eul=dcm2euler_v000(Cbn);
        fprintf(FOUT, '%12.12f ', [pr_ctr;Llh;vel_n;eul;Ximu;Xclk]);
        fprintf(FOUT, '\n');
        fprintf(FCOV, '%12.12f', [diag(P).^0.5]);
        fprintf(FCOV, '\n');
    end

    %Apply the GPS Pseudorange at 1Hz
    if (~mod(pr_ctr,400))
        gps_data=get_gps_data(FGPS);    %gps_data(i,:)=[sat_prn,pseudo distance,sat_x,sat_y,sat_z];

        %Start Sequential Processing of PseudoRanges
        dx=zeros(nst,1);
        for (in=1:size(gps_data,1))
            %Compute the innovation
            nom_range=norm(gps_data(in,3:5)'-pos_e);
            inno=gps_data(in,2)-(nom_range+Cclk*Xclk);

            %Observation Matrix
            H=zeros(1,nst);
            H(1,1:3)=(gps_data(in,3:5)-pos_e')/nom_range;
            H(1,16:17)=Cclk;

            Robs=ObsErrDef.sR*ObsErrDef.sR';

            %Update
            K=(P*H')/(H*P*H'+Robs);
            P=(eye(nst)-K*H)*P;
            dx=dx+K*(inno-H*dx);
        end

        %Correct states
        pos_e=pos_e-dx(1:3);
        vel_e=vel_e-dx(4:6);
        mx_a=eye(3)+skew(dx(7:9));
        Cbe=mx_a*Cbe;
        Ximu=Ximu+dx(10:15);
        Xclk=Xclk+dx(16:17);
    end

    %Read the next imu data from file
    imu_data=fscanf(FIMU, '%f', 6);
end

fclose(FIMU);
fclose(FGPS);
fclose(FCOV);
fclose(FOUT);

return;

%%plot the results
load([DIRPATH  'output.txt']);
plot(output(:,2),output(:,3));


classdef nndagnn < nntest

  properties
    net;
    x;
    class;
    inputs;
    outputs;
  end

  methods (TestClassSetup)
    function initNet(test, device)
      test.net = [];
      test.net.layers = {} ;
      test.net.layers{end+1} = struct('type', 'conv', ...
        'weights', {{test.randn(5,5,1,20), test.zeros(1, 20)}}, ...
        'stride', 1, ...
        'pad', 0) ;
      test.net.layers{end+1} = struct('type', 'pool', ...
        'method', 'max', ...
        'pool', [2 2], ...
        'stride', 2, ...
        'pad', 0) ;
      test.net.layers{end+1} = struct('type', 'conv', ...
        'weights', {{test.randn(5,5,20,50),test.zeros(1,50)}}, ...
        'stride', 1, ...
        'pad', 0) ;
      test.net.layers{end+1} = struct('type', 'pool', ...
        'method', 'max', ...
        'pool', [2 2], ...
        'stride', 2, ...
        'pad', 0) ;
      test.net.layers{end+1} = struct('type', 'conv', ...
        'weights', {{test.randn(4,4,50,500),test.zeros(1,500)}}, ...
        'stride', 1, ...
        'pad', 0) ;
      test.net.layers{end+1} = struct('type', 'relu') ;
      test.net.layers{end+1} = struct('type', 'conv', ...
        'weights', {{test.randn(1,1,500,10),test.zeros(1,10)}}, ...
        'stride', 1, ...
        'pad', 0) ;
      test.net.layers{end+1} = struct('type', 'softmaxloss') ;
      % Fill the missing values
      test.net = dagnn.DagNN.fromSimpleNN(vl_simplenn_tidy(test.net));
      test.inputs = test.net.getInputs();
      test.outputs = test.net.getOutputs();

      test.x = test.randn(32, 32, 1, 20) ;
      test.class = test.toDevice(randi(10, 20, 1));
      test.net.move(device) ;
    end
  end

  methods (Test)
    function simpleRun(test)
      % Verify the forget functionality for the forward pass
      test.net.conserveMemory = true;
      test.forward();
      for ri = 1:numel(test.net.vars)
        if ismember(test.net.vars(ri).name, test.outputs)
          test.verifyNotEmpty(test.net.vars(ri).value);
        else
          test.verifyEmpty(test.net.vars(ri).value);
        end
      end
      % Verify the forget functionality for the backward pass
      test.backward();
      for ri = 1:numel(test.net.vars)
        if ismember(test.net.vars(ri).name, test.inputs)
          test.verifyNotEmpty(test.net.vars(ri).value);
        else
          test.verifyEmpty(test.net.vars(ri).value);
        end
      end
      for pi = 1:numel(test.net.params)
        test.verifyNotEmpty(test.net.params(pi).der);
      end
    end

    function conserveMemory(test)
      % Verify the forget functionality for the forward pass
      test.net.conserveMemory = false;
      test.forward();
      for ri = 1:numel(test.net.vars)
          test.verifyNotEmpty(test.net.vars(ri).value);
      end
      % Verify the forget functionality for the backward pass
      test.backward();
      for ri = 1:numel(test.net.vars)
        test.verifyNotEmpty(test.net.vars(ri).value);
        if ~ismember(test.net.vars(ri).name, test.inputs)
          test.verifyNotEmpty(test.net.vars(ri).der);
        end
      end
    end

    function precious(test)
      % Verify that the precious argument works
      selLayer = 3;
      outputIdx = test.net.layers(selLayer).outputIndexes;
      test.net.vars(outputIdx).precious = true;
      test.net.conserveMemory = true;
      test.forward();
      for ri = 1:numel(test.net.vars)
        if ismember(test.net.vars(ri).name, test.outputs) || ri == outputIdx
          test.verifyNotEmpty(test.net.vars(ri).value);
        else
          test.verifyEmpty(test.net.vars(ri).value);
        end
      end
      test.net.vars(outputIdx).precious = false;
    end
  end

  methods
    function forward(test)
      test.net.reset();
      test.net.eval({'x0', test.x, 'label', test.class});
    end
    function backward(test)
      test.net.reset();
      test.net.eval({'x0', test.x, 'label', test.class}, {'x8', 1});
    end
  end
end


function epsilon = epsm(units)

    %EPSM  Calculate the accuracy of the map computations
    %
    %  e = EPSM returns the accuracy of computations performed in
    %  the Mapping Toolbox.  The accuracy returned is in degrees.
    %
    %  e = EPSM('units') returns the accuracy in the units specified
    %  by the string 'units'.  If omitted, 'degrees' are assumed.
    %
    %  See also EPS

    %  Copyright 1996-2000 Systems Planning and Analysis, Inc. and The MathWorks, Inc.
    %  Written by:  E. Byrns, E. Brown
    %   $Revision: 1399 $    $Date: 2006-08-11 11:19:27 +0200 (Fr, 11 Aug 2006) $

    %  Define the limit in degrees

    %report_this_filefun(mfilename('fullpath'));

    degepsilon = 1.0E-6;

    if nargin == 0
        epsilon = degepsilon;   return

        %  Speed up function with special unit string tests

    elseif strcmp(units,'degrees')
        epsilon = degepsilon;   return

    elseif strcmp(units,'radians')
        epsilon = degepsilon*pi/180;   return

    else
        [units,msg] = unitstr(units,'angles');
        if ~isempty(msg);   error(msg);   end
        epsilon = angledim(degepsilon,'degrees',units);
    end
end


function tran03_values_test ( )

%*****************************************************************************80
%
%% TRAN03_VALUES_TEST demonstrates the use of TRAN03_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRAN03_VALUES_TEST:\n' );
  fprintf ( 1, '  TRAN03_VALUES stores values of\n' );
  fprintf ( 1, '  the transportation function of order 3.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X               FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = tran03_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


function acoustic_feat_ex( data_dir, save_dir )

rng(0);
%% Directory setting

% system(['rm -rf ', save_dir]);
%
% system(['mkdir ', save_dir]);
system(['mkdir ', save_dir, '/Normalize_Factor']);
system(['mkdir ', save_dir, '/Labels']);

%% Parameter setting

audio_sr = 16000;
split_num = 1;
name_mrcg = [save_dir, '/se_mrcg'];
name_label = [save_dir, '/se_label'];

audio_list = getAllFiles(data_dir, 'FileFilter', '\.wav$');
label_list = getAllFiles(data_dir, 'FileFilter', '\.mat$');

winlen             = ceil(audio_sr*25*0.001);	%window length (default : 25 ms)
winstep            = ceil(audio_sr*10*0.001);	%window step (default : 10 ms)

train_mean = 0;
train_std = 0;

for i = 1:1:length(audio_list)
    clc
    fprintf("MRCG extraction %d/%d ...\n", i, length(audio_list));
    %% Read audio

    noisy_speech = audioread(audio_list{i});  % noisy_speech load
    noisy_speech = noisy_speech(1:(length(noisy_speech)-mod(length(noisy_speech), split_num)));
    noisy_speech = reshape(noisy_speech, [], split_num);

    %% Caliculate MRCG
    mrcg = cell(split_num, 1);

    for j = 1:1:split_num
        mrcg{j, 1} = MRCG_features(noisy_speech(:, j), audio_sr)';
        %     imagesc(s(20000:20500,:)*1000)
    end

    mrcg_mat = cell2mat(mrcg);

    size(mrcg_mat)
    %% Save normalization factor

    temp_mean = mean(mrcg_mat,1);
    temp_std = std(mrcg_mat,1,1);
    save([save_dir, '/Normalize_Factor/normalize_factor_', sprintf('%3.3d', i)],'temp_mean', 'temp_std');
    train_mean = temp_mean + train_mean;
    train_std = temp_std + train_std;

    %% Read label
    label = cell2mat(struct2cell(load(label_list{i})));  % label load

    %% Save framed label & MRCG
    framed_label = Truelabel2Trueframe( label, winlen, winstep );
    length(framed_label)
    if (length(mrcg_mat) > length(framed_label))
        binary_saver( name_mrcg, mrcg_mat(1:length(framed_label), :), i );
        binary_saver( name_label, framed_label, i );
    else
        binary_saver( name_mrcg, mrcg_mat, i );
        binary_saver( name_label, framed_label(1:length(mrcg_mat), 1), i );
    end
end

disp('MRCG extraction done.')
%% Save global normalization factor

global_mean = train_mean / length(audio_list);
global_std = train_std / length(audio_list);
save([save_dir, '/global_normalize_factor'], 'global_mean', 'global_std');

%% Move label data

feat_list = getAllFiles(save_dir);

for i=1:1:length(feat_list)
    if ~isempty(strfind(feat_list{i}, 'label'))
        [pathstr, name, ext] = fileparts(feat_list{i});
        new_path = [pathstr, '/Labels/', name, ext];
        movefile(feat_list{i}, new_path);
    end
end

end


% ========================================================================
% Fast Multi-Scale Structural Patch Decomposition for Multi-Exposure Image Fusion, TIP,2020
% algorithm Version 1.0
% Copyright(c) 2020, Hui Li, Kede Ma, Yongwei Yong and Lei Zhang
% All Rights Reserved.
% ----------------------------------------------------------------------
% Permission to use, copy, or modify this software and its documentation
% for educational and research purposes only and without fee is hereby
% granted, provided that this copyright notice and the original authors'
% names appear on all copies and supporting documentation. This program
% shall not be used, rewritten, or adapted as the basis of a commercial
% software or hardware product without first obtaining permission of the
% authors. The authors make no representations about the suitability of
% this software for any purpose. It is provided "as is" without express
% or implied warranty.
%----------------------------------------------------------------------
% Please refer to the following paper:
% H. Li et al., "Fast Multi-Scale Structural Patch Decomposition for Multi-Exposure Image Fusion, 2020" In press
% IEEE Transactions on Image Processing
% Please kindly report any suggestions or corrections to xiaohui102788@126.com
%----------------------------------------------------------------------

function [fI3, i_meant,aa,N1] = scale_interm(imgSeqColor,r)

[h,w,n]=size(imgSeqColor);
N = boxfilter(ones(h, w), r);

tem=ones(h, w);
tem(:,2:2:w)=0;
tem(2:2:h,:)=0;
N1= boxfilter(tem, r);

p=4;

[WD, Cmax,i_mean2]= weight_cal_detail(imgSeqColor,p,r);
WD=WD.*repmat(Cmax,[1 1 n]);

% WD= weight_cal_gray(imgSeqColor,[0 0 0 1 1 0 0]);

% WB= weight_cal_gray(imgSeqColor,[0 0 0 0 0 1 0]);
% WD=WB;

F_temp2_detail=zeros(h,w,n);
% F_temp2_base=zeros(h,w,n);

%% approximate aggregation through averaging(mean filter) the weight map

i_meant=zeros(ceil(h/2),ceil(w/2),2);
% tic
for i = 1:n

    aa=i_mean2(:,:,i).*tem;
    i_meant(:,:,i)=aa(1:2:h,1:2:w);
    %      i_meant2(:,:,i)=i_meant(:,:,i).*(1-edg(:,:,i));

    %     W_D1=boxfilter(aa.*WD(:,:,i), r)./ N;
    %     W_D2=boxfilter(tem.*WD(:,:,i), r)./ N;

    W_D1=boxfilter(i_mean2(:,:,i).*WD(:,:,i), r)./ N;
    W_D2=boxfilter(WD(:,:,i), r)./ N;

    F_temp2_detail(:,:,i)=W_D2.*(imgSeqColor(:,:,i))-W_D1;

end

fI3=sum(F_temp2_detail,3);
% toc

%figure,imshow(mat2gray(fI3))

end


%system model for wander mechanization (with dcm) under small angle
%assumption and no wander error. (Based on Phi-model definition)
%dt==0 -> return cont. time model
%Depending on input arguments this function may return (A,N), (Ad,N),
%(Ac,Qc),(Ad,Qd)
%this function is nothing but sys_wander_large with deleted wander states.

%Although this is for a wander-azimuth mechanization, this implementation
%is singular as it calls geoparam_v000.

%For a real non-singular sys-model implementation see sys_wander_psi_v000

%(For an example usage of this see: example_largeheading)

function [RetA RetB]=sys_wander_dcm(llh, vel_n, Cbn, wander, acc, dt, Aimu, Qimu, Cimu, Rimu)
% position   (1-3) (not it llh but in northing, easting, height)
% velocity   (4-6)
% attitude   (7-9)
% wander     (10-11) ([del_sa, del_ca])

%system disturbance coefs
N=zeros(9,6);
N(7:9,4:6)=-Cbn; %attitude
N(4:6,1:3)=Cbn; %velocity

%system matrix
A=zeros(9);

[Rn, Re, g, sL, cL, WIE_E]=geoparam_v000(llh);
Cgn=[wander(2) wander(1) 0;-wander(1) wander(2) 0; 0 0 1];
Rn_h=Rn+llh(3);
Re_h=Re+llh(3);
R=sqrt(Rn_h*Re_h);
acc_n=Cbn*acc;

mx_b=[wander(2)*(-WIE_E)*sL/Rn_h 0;wander(1)*(-WIE_E)*sL/Rn_h 0; (-WIE_E)*cL/Rn_h 0];
mx_c=[0 1/R 0;-1/R 0 0;0 0 0];
mx_d=[-vel_n(2)/R^2; vel_n(1)/R^2;0];

mx_e=Cgn*[0 1/(Re_h) 0; -1/(Rn_h) 0 0;0 0 0]*Cgn'*vel_n+2*Cgn*[WIE_E*cL; 0; -WIE_E*sL]; %2wie_n+wen_n
mx_f=Cgn*[0 1/(Re_h) 0; -1/(Rn_h) 0 0;0 0 0]*Cgn'*vel_n+Cgn*[WIE_E*cL; 0; -WIE_E*sL]; %wie_n+wen_n

%%position errors
A(1:2,4:5)=Cgn(1:2,1:2)';
A(3,6)=-1;

%%Velocity errors
A(4:6,1:2)=2*skew(vel_n)*mx_b;
A(4:6,3)=skew(vel_n)*mx_d+[0;0;-2*g/R];
A(4:6,4:6)=skew(vel_n)*mx_c-skew(mx_e);
A(4:6,7:9)=skew(acc_n);

%%Attitude errors
A(7:9,1:2)=mx_b;
A(7:9,3)=mx_d;
A(7:9,4:6)=mx_c;
A(7:9,7:9)=-skew(mx_f);

%%%%%%%%
%%Combine navigation models with imu models and return
if ~(isempty(Aimu) || isempty(Qimu) || isempty(Rimu) || isempty(Cimu))
    nst_imu=size(Aimu,1);
    nst_nav=size(A,1);
    Ac=[A N*Cimu;zeros(nst_imu,nst_nav) Aimu];
    Qc=[N*Rimu*N' zeros(nst_nav,nst_imu);zeros(nst_imu,nst_nav) Qimu];

    RetA=Ac;
    RetB=Qc;

    if (dt>0)
        %Discretize the model
        nst=nst_nav+nst_imu;
        mx_a = dt*[-Ac,Qc;zeros(nst),Ac'];
        mx_b = expm(mx_a);
        STM = mx_b(nst+1:2*nst,nst+1:2*nst)';
        Qd = STM*mx_b(1:nst,nst+1:2*nst);
        RetA=STM;
        RetB=Qd;
    end
else
    RetA=A;
    RetB=N;
    if (dt>0)
        RetA=expm(A*dt);
    end
end


function out = get_scale_sample(im, pos, base_target_sz, scaleFactors, scale_window, scale_model_sz)

% out = get_scale_sample(im, pos, base_target_sz, scaleFactors, scale_window, scale_model_sz)
%
% Extracts a sample for the scale filter at the current
% location and scale.

nScales = length(scaleFactors);

for s = 1:nScales
    patch_sz = floor(base_target_sz * scaleFactors(s));

    xs = floor(pos(2)) + (1:patch_sz(2)) - floor(patch_sz(2)/2);
    ys = floor(pos(1)) + (1:patch_sz(1)) - floor(patch_sz(1)/2);

    % check for out-of-bounds coordinates, and set them to the values at
    % the borders
    xs(xs < 1) = 1;
    ys(ys < 1) = 1;
    xs(xs > size(im,2)) = size(im,2);
    ys(ys > size(im,1)) = size(im,1);

    % extract image
    im_patch = im(ys, xs, :);

    % resize image to model size
    im_patch_resized = imResample(im_patch, scale_model_sz);

    % extract scale features
    temp_hog = fhog(single(im_patch_resized), 4);
    temp = temp_hog(:,:,1:31);

    if s == 1
        out = zeros(numel(temp), nScales, 'single');
    end

    % window
    out(:,s) = temp(:) * scale_window(s);
end