function [ n_data, a, b, x, fx ] =  hyper_1f1_values ( n_data )

%*****************************************************************************80
%
%% HYPER_1F1_VALUES returns some values of the hypergeometric function 1F1.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      fx = Hypergeometric1F1 [ a, b, x ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 March 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Cambridge University Press, 1999,
%    ISBN: 0-521-64314-7,
%    LC: QA76.95.W65.
%
%    Daniel Zwillinger, editor,
%    CRC Standard Mathematical Tables and Formulae,
%    30th Edition,
%    CRC Press, 1996,
%    ISBN: 0-8493-2479-3,
%    LC: QA47.M315.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0
%    before the first call.  On each call, the routine increments N_DATA by 1,
%    and returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real A, B, X, the parameters.
%
%    Output, real FX, the value of the function.
%
  n_max = 24;

  a_vec = [ ...
    -2.500, ...
    -0.500, ...
     0.500, ...
     2.500, ...
    -2.500, ...
    -0.500, ...
     0.500, ...
     2.500, ...
    -2.500, ...
    -0.500, ...
     0.500, ...
     2.500, ...
     0.825, ...
     1.100, ...
     1.650, ...
     3.300, ...
     0.825, ...
     1.100, ...
     1.650, ...
     3.300, ...
     0.825, ...
     1.100, ...
     1.650,...
     3.300 ];
  b_vec = [ ...
     3.3, ...
     1.1, ...
     1.1, ...
     3.3, ...
     3.3, ...
     1.1, ...
     1.1, ...
     3.3, ...
     3.3, ...
     1.1, ...
     1.1, ...
     3.3, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7 ];
  fx_vec = [ ...
     0.81879926689265186854, ...
     0.88283984828032972070, ...
     1.1245023764952626690, ...
     1.2101049301639599598, ...
     0.12723045536781567174, ...
     0.12326016871544045107, ...
     2.3297954665128293051, ...
     3.3890020264468009733, ...
    -0.18819510282516768874, ...
    -1.0764203806547022727, ...
     5.7521824680907968433, ...
     9.9998567403304086593, ...
     1.0317208964319891384, ...
     1.0424867029249952040, ...
     1.0643112000949092012, ...
     1.1321844369742336326, ...
     1.2328402688568452181, ...
     1.3200654482027340732, ...
     1.5104811522310825217, ...
     2.2307520785940524365, ...
     1.5197286298183137741, ...
     1.7364938170250847619, ...
     2.2492330307668135926, ...
     4.6377737119178965298 ];
  x_vec = [ ...
     0.25, ...
     0.25, ...
     0.25, ...
     0.25, ...
     1.55, ...
     1.55, ...
     1.55, ...
     1.55, ...
     2.85, ...
     2.85, ...
     2.85, ...
     2.85, ...
     0.25, ...
     0.25, ...
     0.25, ...
     0.25, ...
     1.55, ...
     1.55, ...
     1.55, ...
     1.55, ...
     2.85, ...
     2.85, ...
     2.85, ...
     2.85 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    a = 0.0;
    b = 0.0;
    x = 0.0;
    fx = 0.0;
  else
    a = a_vec(n_data);
    b = b_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function check = f_check ( m, n )

%*****************************************************************************80
%
%% F_CHECK checks the parameters of the F PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 August 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the parameters of the PDF.
%    0 < M
%    0 < N
%
%    Output, logical CHECK, is TRUE if the parameters are legal.
%
  if ( m ~= round ( m ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'F_CHECK - Fatal error!\n' );
    fprintf ( 1, '  M is not an integer.\n' );
    check = 0;
  end

  if ( m <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'F_CHECK - Fatal error!\n' );
    fprintf ( 1, '  M <= 0.\n' );
    check = 0;
  end

  if ( n ~= round ( n ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'F_CHECK - Fatal error!\n' );
    fprintf ( 1, '  N is not an integer.\n' );
    check = 0;
  end

  if ( n <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'F_CHECK - Fatal error!\n' );
    fprintf ( 1, '  N <= 0.\n' );
    check = 0;
  end

  check = 1;

  return
end


function Y = scale(X,S,dims)
%SCALE Scale along specified dimensions of tensor.
%
%   Y = SCALE(X,S,DIMS) scales the tensor X along the dimension(s)
%   specified in DIMS using the scaling data in S. If DIMS contains
%   only one dimension, then S can be a column vector. Otherwise, S
%   should be a tensor.
%
%   Examples
%   X = tenones([3,4,5]);
%   S = 10 * [1:5]'; Y = scale(X,S,3)
%   S = tensor(10 * [1:5]',5); Y = scale(X,S,3)
%   S = tensor(1:12,[3 4]); Y = scale(X,S,[1 2])
%   S = tensor(1:12,[3 4]); Y = scale(X,S,-3)
%   S = tensor(1:60,[3 4 5]); Y = scale(X,S,1:3)
%
%   See also TENSOR, TENSOR/COLLAPSE.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

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

dims = tt_dimscheck(dims,ndims(X));
remdims = setdiff(1:ndims(X),dims);

% Convert to a matrix so that each column of A can be scaled by a
% vectorized version of S.
A = double(tenmat(X,dims,remdims));

switch(class(S))
    case {'tensor'}
        if ~isequal(size(S), X.size(dims))
            error 'Size mismatch';
        end
        % Vectorize S.
        S = double(tenmat(S,1:ndims(S),[]));
    case {'double'}
        if size(S,1) ~= X.size(dims)
            error 'Size mismatch';
        end
    otherwise
        error('Invalid scaling factor');
end

[m,n] = size(A);

% If the size of S is pretty small, we can convert it to a diagonal matrix
% and multiply by A. Otherwise, we scale A column-by-column.
if (m <= n)
    B = diag(S) * A;
else
    B = zeros(size(A));
    for j = 1:n
        B(:,j) = S .* A(:,j);
    end
end

% Convert the matrix B back into a tensor and return.
Y = tensor(tenmat(B,dims,remdims,X.size));


function u=glotros(d,t,p)
%GLOTROS  Rosenberg glottal model U=(D,T,P)
% d is derivative of flow waveform
% t is in fractions of a cycle
% p has parameters
%	p(1)=closure time
%	p(2)=+ve/-ve slope ratio

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: glotros.m,v 1.4 2007/05/04 07:01:38 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin < 2
  tt=(0:99)'/100;
else
  tt=mod(t,1);
end
u=zeros(size(tt));
de=[0.6 0.5]';
if nargin < 3
  p=de;
elseif length(p)<2
  p=[p(:); de(length(p)+1:2)];
end
pp=p(1)/(1+p(2));
ta=tt<pp;
tb=tt<p(1) & ~ta;
wa=pi/pp;
wb=0.5*pi/(p(1)-pp);
fb=wb*pp;
if d==0
  u(ta)=0.5*(1-cos(wa*tt(ta)));
  u(tb)=cos(wb*tt(tb)-fb);
elseif d==1
  u(ta)=0.5*wa*sin(wa*tt(ta));
  u(tb)=-wb*sin(wb*tt(tb)-fb);
elseif d==2
  u(ta)=0.5*wa^2.*cos(wa*tt(ta));
  u(tb)=-wb^2*cos(wb*tt(tb)-fb);
else
  error('Derivative must be 0,1 or 2');
end


%
% [Sin,SNin] = Sin_vsh_ctf_fast(r_sphere,R,EX,EY,EZ,Lin)
%
% Calculate the internal SSS basis Sin for a CTF system
% using vector spherical harmonics
%
function [Sin,SNin] = Sin_vsh_ctf_fast(r_sphere,R,EX,EY,EZ,Lin)

% Copyright (c) 2016, Elekta Oy
% ---------------------------------------
%
% Redistribution and use of the Software in source and binary forms, with or without
% modification, are permitted for non-commercial use.
%
% The Software is provided "as is" without warranties of any kind, either express or
% implied including, without limitation, warranties that the Software is free of defects,
% merchantable, fit for a particular purpose. Developer/user agrees to bear the entire risk
% in connection with its use and distribution of any and all parts of the Software under this license.
%

mu0 = 1.25664e-6; % Permeability of vacuum
%
% For numerical surface integration:
%
%baseline = 50e-3;
dx = 4.5e-3;
dy = 4.5e-3;
dz1 = 0;
dz2 = 50e-3;
D = [dx dy dz1; dx -dy dz1; -dx dy dz1; -dx -dy dz1; dx dy dz2; dx -dy dz2; -dx dy dz2; -dx -dy dz2]';
for j = 1:8
   if j <= 4
      weights(j) = 1/(4*1);
   else
      weights(j) = -1/(4*1);
   end
end
weights = weights';

for ch = 1:size(R,2)
   count = 1;
   R(:,ch) = R(:,ch) - r_sphere;
   Sin(ch,:) = -mu0*vsh_response(R(:,ch),EX(:,ch),EY(:,ch),EZ(:,ch),D,weights,Lin);
end
for j = 1:size(Sin,2)
   SNin(:,j) = Sin(:,j)/norm(Sin(:,j));
end

function Sin_elements = vsh_response(r,ex,ey,ez,D,weights,Lin)

for j = 1:length(weights)
    r_this = r + D(1,j)*ex + D(2,j)*ey;
    rn(j) = norm(r_this);
    theta(j) = acos(r_this(3)/rn(j));
    phi(j) = atan2(r_this(2),r_this(1));
    sint(j) = sin(theta(j));
    sinp(j) = sin(phi(j));
    cost(j) = cos(theta(j));
    cosp(j) = cos(phi(j));
    for l = 1:Lin
       p0{l}(:,j) = legendre(l,cos(theta(j)));
       rnv(j,l) = rn(j)^(l+2);
   end
end
Sin_elements = [];
for l = 1:Lin
  for m = -l:l
    for j = 1:length(weights)
      %vs = vsh_modified_in_fast(theta(j),phi(j),l,m,p0{l}(:,j))'/rn(j)^(l+2);
      vs = vsh_modified_in_fast(theta(j),phi(j),l,m,p0{l}(:,j))'/rnv(j,l);
      V(1,j) = vs(1)*sint(j)*cosp(j) + vs(2)*cost(j)*cosp(j) - vs(3)*sinp(j);
      V(2,j) = vs(1)*sint(j)*sinp(j) + vs(2)*cost(j)*sinp(j) + vs(3)*cosp(j);
      V(3,j) = vs(1)*cost(j) - vs(2)*sint(j);
    end
    Sin_elements = [Sin_elements dot(V*weights,ez)]; % Cartesian coordinates
                                                     %Sin_element = Sin_element/sqrt((l+1)*(2*l+1));  % Back to orthonormal presentation
  end
end
%Sin_element = dot(V*weights,ez); % Cartesian coordinates
%Sin_element = Sin_element/sqrt((l+1)*(2*l+1));  % Back to orthonormal presentation


function kern = nddisimKernParamInit(kern)

% NDDISIMKERNPARAMINIT NDDISIM kernel parameter initialisation.
% The driven input single input motif (DISIM) kernel is specifically designed for
% working with gene networks where there is assumed to be a single
% transcription factor controlling several genes. This transcription
% factor, in turn, is driven by its own gene's RNA. The model takes the
% following form: each gene is
% related to the transcription factor through the following
% differential equation,
%
% dx(t)/dt = B + C f(t-delta) - D x(t),
%
% where D is a decay term, C is a response term, delta is a time delay
% and B is an initial level. Then if f(t) is assumed to be the result of
% a further differential equation,
%
% df(t)/dt = Sx'(t) - D' x'(t)
%
% where x'(t) is assumed to come from a Gaussian process with an RBF
% covariance function f(t) is a Gaussian process with a covariance function
% provided by the single input motif kernel (SIM) and x(t) is a Gaussian
% process with covariance function provided by this kernel, the DISIM kernel.
%
% The kernel is designed to interoperate with the multiple output
% block kernel so that f(t) can be inferred given several different
% instantiations of x(t) (associated with different genes).
%
% The parameters (B, C, delta, S, D and D') are constrained positive.
%
% FORMAT
% DESC initialises the single input motif
%  kernel structure with some default parameters.
% ARG kern : the kernel structure which requires initialisation.
% RETURN kern : the kernel structure with the default parameters placed in.
%
% SEEALSO : kernCreate, kernParamInit, simKernCompute
%
% COPYRIGHT : Neil D. Lawrence, 2006
%
% COPYRIGHT : Antti Honkela, 2007, 2009
%
% COPYRIGHT: Jaakko Peltonen, 2011

% KERN

if kern.inputDimension > 1
  error('NDDISIM kernel is only valid for one-dimensional input.')
end

if isfield(kern, 'options') && isfield(kern.options, 'gaussianInitial') && ...
      kern.options.gaussianInitial,
  kern.gaussianInitial = 1;
  kern.initialVariance = 1;
else
  kern.gaussianInitial = 0;
end

kern.inverseWidth = 1;
kern.di_variance = 1;
kern.decay = 1;
kern.variance = 1;
kern.delay = 1e-7;

if kern.gaussianInitial,
  kern.nParams = 6;
else
  kern.nParams = 5;
end

if isfield(kern, 'options') && isfield(kern.options, 'paramTransform'),
  paramTransform = kern.options.paramTransform;
else
  paramTransform = 'sigmoidab';
end

switch paramTransform,
 case 'sigmoidab',
  for k=1:kern.nParams,
    kern.transforms(k).index = k;
    kern.transforms(k).type = 'sigmoidab';
    kern.transforms(k).transformsettings = [0 1e6];
  end;
 case 'bounded',
  for k=1:kern.nParams,
    kern.transforms(k).index = k;
    kern.transforms(k).type = optimiDefaultConstraint('bounded');
    kern.transforms(k).transformsettings = [0 1e6];
  end;
 case 'identity',
  for k=1:kern.nParams,
    kern.transforms(k).index = k;
    kern.transforms(k).type = 'identity';
    kern.transforms(k).transformsettings = [0 1e6];
  end;
 case 'positive',
  for k=1:kern.nParams,
    kern.transforms(k).index = k;
    kern.transforms(k).type = optimiDefaultConstraint('positive');
  end;
 case 'none',
 otherwise,
  error('Unknown paramTransform');
end

kern.isStationary = false;


function [ centers, mincenter, mindist, q2, quality ] = kmeans_fast ( ...
  data, initcenters, method )

%*****************************************************************************80
%
%% KMEANS_FAST carries out the fast KMEANS algorithm of Charles Elkan.
%
%  Discussion:
%
%    Note that if the input quantity INITCENTERS is a scalar, it is taken
%    to be the number of clusters desired, but if it is a KxM array, it is]
%    taken to be the M-dimensional coordinates of K points, to be used as
%    initial guesses for the cluster centers.
%
%  Modified:
%
%    04 September 2013
%
%  Author:
%
%    Charles Elkan
%
%  Reference:
%
%    Charles Elkan,
%    Using the Triangle Inequality to Accelerate k-Means,
%    Proceedings of the Twentieth International Conference on Machine Learning
%    (ICML-2003),
%    Washington DC, 2003.
%
%  Parameters:
%
%    Input, real DATA(N,M), the M-dimensional coordinates of N points.
%
%    Input, integer INITCENTERS, the number K of clusters desired.
%
%    Input, real INITCENTERS(K,M), the M-dimensional coordinates of K
%    points, which the program will use as starting guesses for the
%    cluster centers.
%
%    Input, integer METHOD, selects the algorithm to be used.
%    * 0, unoptimized, using n by k matrix of distances O(nk) space;
%    * 1, vectorized, using only O(n+k) space;
%    * 2, like 1, in addition using distance inequalities (default).
%
%    Output, real CENTERS(K,M), the M-dimensional coordinates of the K
%    cluster centers.
%
%    Output, integer MINCENTER(N,1), the index, for each data point, of
%    the cluster to which it belongs.
%
%    Output, real MINDIST(N,1), an upper bound of the distance of each point to
%    the nearest center.
%
%    Output, real Q2, the mean of UDIST^2.
%
%    Output, real QUALITY, the mean of UDIST.

tic
if nargin < 3 method = 2; end
[n,dim] = size(data);

if max(size(initcenters)) == 1
    k = initcenters;
    [centers, mincenter, mindist, lower, computed] = anchors(mean(data),k,data);
    total = computed;
    skipestep = 1;
else
    centers = initcenters;
    mincenter = zeros(n,1);
    total = 0;
    skipestep = 0;
    [k,dim2] = size(centers);
    if dim ~= dim2 error('dim(data) ~= dim(centers)'); end;
end

nchanged = n;
iteration = 0;
oldmincenter = zeros(n,1);

while nchanged > 0
    % do one E step, then one M step
    computed = 0;

    if method == 0 & ~skipestep
        for i = 1:n
            for j = 1:k
                distmat(i,j) = calcdist(data(i,:),centers(j,:));
            end
        end
        [mindist,mincenter] = min(distmat,[],2);
        computed = k*n;

    elseif (method == 1 | (method == 2 & iteration == 0)) & ~skipestep
        mindist = Inf*ones(n,1);
        lower = zeros(n,k);
        for j = 1:k
           jdist = calcdist(data,centers(j,:));
           lower(:,j) = jdist;
           track = find(jdist < mindist);
           mindist(track) = jdist(track);
           mincenter(track) = j;
        end
        computed = k*n;

    elseif method == 2 & ~skipestep
        computed = 0;
%
% for each center, nndist is half the distance to the nearest center
% if d(x,center) < nndist then x cannot belong to any other center
% mindist is an upper bound on the distance of each point to its nearest center
%
        nndist = min(centdist,[],2);
% the following usually is not faster
%        ldist = min(lower,[],2);
%        mobile = find(mindist > max(nndist(mincenter),ldist));
        mobile = find(mindist > nndist(mincenter));

% recompute distances for point i and center j
%       only if j can possibly be the new nearest center
% for speed, the first check has been optimized by modifying centdist
% swapping the order of the checks is slower for data with natural clusters

        mdm = mindist(mobile);
        mcm = mincenter(mobile);

        for j = 1:k
% the following is incorrect: for j = unique(mcm)'
            track = find(mdm > centdist(mcm,j));
            if isempty(track) continue; end
            alt = find(mdm(track) > lower(mobile(track),j));
            if isempty(alt) continue; end
            track1 = mobile(track(alt));
%
% calculate exact distances to the mincenter
% recalculate separately for each jj to avoid copying too much of data
% redo may be empty, but we don't need to check this.
%
            redo = find(~recalculated(track1));
            redo = track1(redo);
            c = mincenter(redo);
            computed = computed + size(redo,1);
            for jj = unique(c)'
                rp = redo(find(c == jj));
                udist = calcdist(data(rp,:),centers(jj,:));
                lower(rp,jj) = udist;
                mindist(rp) = udist;
            end
            recalculated(redo) = 1;

            track2 = find(mindist(track1) > centdist(mincenter(track1),j));
            track1 = track1(track2);
            if isempty(track1) continue; end

            % calculate exact distances to center j
            track4 = find(lower(track1,j) < mindist(track1));
            if isempty(track4) continue; end
            track5 = track1(track4);
            jdist = calcdist(data(track5,:),centers(j,:));
            computed = computed + size(track5,1);
            lower(track5,j) = jdist;

            % find which points really are assigned to center j
            track2 = find(jdist < mindist(track5));
            track3 = track5(track2);
            mindist(track3) = jdist(track2);
            mincenter(track3) = j;
        end % for j=1:k
    end % if method

    oldcenters = centers;
%
% M step: recalculate the means for each cluster
% if a cluster is empty, its mean is left unchanged
% we minimize computations for clusters with little changed membership
%
    diff = find(mincenter ~= oldmincenter);
    diffj = unique([mincenter(diff);oldmincenter(diff)])';
    diffj = diffj(find(diffj > 0));

    if size(diff,1) < n/3 & iteration > 0
         for j = diffj
            plus = find(mincenter(diff) == j);
            minus = find(oldmincenter(diff) == j);
            oldpop = pop(j);
            pop(j) = pop(j) + size(plus,1) - size(minus,1);
            if pop(j) == 0 continue; end
            centers(j,:) = (centers(j,:)*oldpop + sum(data(diff(plus),:),1) - sum(data(diff(minus),:),1))/pop(j);
        end
    else
        for j = diffj
            track = find(mincenter == j);
            pop(j) = size(track,1);
            if pop(j) == 0 continue; end
% it's correct to have mean(data(track,:),1) but this can make answer worse!
            centers(j,:) = mean(data(track,:),1);
        end
    end

    if method == 2
        for j = diffj
            offset = calcdist(centers(j,:),oldcenters(j,:));
            computed = computed + 1;
            if offset == 0 continue; end
            track = find(mincenter == j);
            mindist(track) = mindist(track) + offset;
            lower(:,j) = max(lower(:,j) - offset,0);
        end
%
% compute distance between each pair of centers
% modify centdist to make "find" using it faster.
%
        recalculated = zeros(n,1);
        realdist = alldist(centers);
        centdist = 0.5*realdist + diag(Inf*ones(k,1));
        computed = computed + k + k*(k-1)/2;
    end

    nchanged = size(diff,1) + skipestep;
    iteration = iteration+1;
    skipestep = 0;
    oldmincenter = mincenter;

%   difference = max(max(abs(oldcenters - centers)));
%   [iteration toc nchanged computed size(diffj,2)]
    fprintf ( 1, '%4d  %g  %d  %d\n', iteration, toc, nchanged, computed );
%   [iteration toc nchanged computed]
    total = total + computed;
end % while nchanged > 0

  udist = calcdist(data,centers(mincenter,:));
  quality = mean(udist);
  q2 = mean(udist.^2);
  %[iteration toc quality q2 total]
  fprintf ( 1, '  %4d  %g  %g  %g  %d\n', iteration, toc, quality, q2, total );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'KMEANS_FAST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end


% set system limits
system = mr.opts('MaxGrad', 10, 'GradUnit', 'mT/m', ...
    'MaxSlew', 50, 'SlewUnit', 'T/m/s', 'rfRingdownTime', 20e-6, ...
    'rfDeadTime', 100e-6, 'adcDeadTime', 10e-6);

% high-level sequence parameters
alpha=90;
sliceThickness=50e-3;
Nx = 2048;
Nrep = 1;
TE=30e-3;
TR = 6000e-3;

rf_90_duration=2.6e-3; % excitation pulse duration
rf_180_duration= 4.6e-3; % refocusing pulse duration

% create and populate the sequence
seq = mr.Sequence(system);              % Create a new sequence object

% Create excitation pulse and gradient
rf_90= mr.makeSLRpulse(pi/2,'duration',rf_90_duration,'timeBwProduct',7.88,'dwell',rf_90_duration/500,'passbandRipple',1,'stopbandRipple',1e-2,'filterType','ms','system',system);

%Create refocusing pulse and gradient
rf_180_1 = mr.makeAdiabaticPulse('wurst','duration',rf_180_duration,'bandwidth',6000,'dwell',rf_180_duration/500,'n_fac',20,'use','refocusing','system',system);

timebwproduct_90=8;   % "experimental" value for the used for SLR excitation
timebwproduct_180=22; % "experimental" value for the used refocusing pulse

grad_amplitude_90 = (timebwproduct_90/2.6e-3)/sliceThickness; %BW/thickness
grad_amplitude_180 = (timebwproduct_180/4.6e-3)/sliceThickness; %BW/thickness

% we cannot use mr.calcDuration(rf_90) and mr.calcDuration(rf_180_1) here because this already includes delays and RF dead times
gx = mr.makeTrapezoid('x','flatTime',rf_90_duration,'amplitude',grad_amplitude_90,'system',system);
gy = mr.makeTrapezoid('y','flatTime',rf_180_duration,'amplitude',grad_amplitude_180,'system',system);
gz = mr.makeTrapezoid('z','flatTime',rf_180_duration,'amplitude',grad_amplitude_180,'system',system);
gz_2 = mr.makeTrapezoid('z','flatTime',rf_180_duration,'amplitude',grad_amplitude_180,'system',system);

% refocusing area for the 90-degree pulse (needs to be remembered in the "refocusing budget")
area_90_toRefocus=gx.amplitude*(0.5*gx.fallTime+gx.flatTime-mr.calcRfCenter(rf_90));

% gradient spoiling
spoilMoment=10/sliceThickness;
gzSpoil=mr.makeTrapezoid('z','area',spoilMoment,'system',system);
gxSpoil=mr.makeTrapezoid('x','duration',2*mr.calcDuration(gzSpoil),'area',spoilMoment,'system',system);
gzSpoil_2=mr.makeTrapezoid('z','duration',2*mr.calcDuration(gzSpoil),'area',spoilMoment,'system',system);

% split x gradient at the end of the plato and calculate the rf_90 delay
gx_parts=mr.splitGradientAt(gx,mr.calcDuration(gx)-gx.fallTime+system.rfRingdownTime);
[gx_p1,rf_90,~]=mr.align('right',gx_parts(1),rf_90,mr.makeDelay(system.rfDeadTime+rf_90_duration+system.rfRingdownTime));
gx_parts(2).delay=0; % set the delay to 0 because it will be used in a separate block

% split y gradient at the end of the plato and calculate the rf_180_1 delay
gy_parts=mr.splitGradientAt(gy,mr.calcDuration(gy)-gy.fallTime+system.rfRingdownTime);
rf_180_1.delay=max(mr.calcDuration(gy_parts(1))-rf_180_duration-system.rfRingdownTime, mr.calcDuration(gzSpoil));
assert(rf_180_1.delay>=system.rfDeadTime);
gy_p1=gy_parts(1);
gy_p1.delay=rf_180_1.delay+rf_180_duration+system.rfRingdownTime-mr.calcDuration(gy_p1);
gy_parts(2).delay=0; % set the delay to 0 because it will be used in a separate block

% combine y gradient with delays to a new gradient the rf_180_2 delay
rf_180_2=rf_180_1;
rf_180_2.delay=max(mr.calcDuration(gxSpoil),mr.calcDuration(gzSpoil_2));
gy_tmp=mr.splitGradientAt(gy,mr.calcDuration(gy)-gy.fallTime);
gy_tmp(1).delay=rf_180_2.delay+rf_180_duration-mr.calcDuration(gy_tmp(1));
gySpoil=mr.makeExtendedTrapezoidArea('y',gy.amplitude,0,spoilMoment+0.5*gy.amplitude*gy.fallTime,system);
gySpoil.delay=mr.calcDuration(gy_tmp(1));
gy_comb=mr.addGradients({gy_parts(2),gy_tmp(1),gySpoil},'system',system);
gy_comb_parts=mr.splitGradientAt(gy_comb,rf_180_2.delay+rf_180_duration+system.rfRingdownTime);%
gy_comb_parts(2).delay=0;

%
gxSpoil_2=mr.makeTrapezoid('x','area',spoilMoment,'system',system);
rf_180_3=rf_180_1;
rf_180_3.delay=mr.calcDuration(gxSpoil_2);
gz.delay=rf_180_3.delay-gz.riseTime;

%Additional Spoiler gradients
gzSpoil_semiFinal=mr.makeExtendedTrapezoidArea('z',0,gz.amplitude,spoilMoment,system);
gxSpoil_semiFinal=mr.makeTrapezoid('x','area',spoilMoment+area_90_toRefocus,'system',system);
gySpoil_semiFinal=mr.makeTrapezoid('y','area',2*spoilMoment,'system',system);
[gxSpoil_semiFinal,gySpoil_semiFinal,gzSpoil_semiFinal]=mr.align('right',gxSpoil_semiFinal,gySpoil_semiFinal,gzSpoil_semiFinal);

rf_180_4=rf_180_1;
rf_180_4.delay=0;
gz_temp=mr.splitGradientAt(gz_2,gz_2.riseTime);
gz_temp(2).delay=0;
gz_parts=mr.splitGradientAt(gz_temp(2),rf_180_4.delay+rf_180_duration);
gz_parts(1).delay=mr.calcDuration(gzSpoil_semiFinal);
rf_180_4.delay=mr.calcDuration(gzSpoil_semiFinal);

gzSpoil_Final=mr.makeExtendedTrapezoidArea('z',gz.amplitude,0,spoilMoment,system);
gxSpoil_Final=mr.makeTrapezoid('x','area',spoilMoment,'system',system);
gySpoil_Final=mr.makeTrapezoid('y','area',spoilMoment,'system',system);

gzSpoil_Final.delay=rf_180_4.delay+rf_180_duration;
gz_comb=mr.addGradients({gzSpoil_semiFinal,gz_parts(1),gzSpoil_Final},'system',system);

gxSpoil_Final.delay=rf_180_4.delay+rf_180_duration;
gySpoil_Final.delay=rf_180_4.delay+rf_180_duration;

gxSpoil_combi=mr.addGradients({gxSpoil_semiFinal,gxSpoil_Final},'system',system);
gySpoil_combi=mr.addGradients({gySpoil_semiFinal,gySpoil_Final},'system',system);

%timing calculation
lTime1=(rf_90_duration+rf_180_duration)/2+system.rfRingdownTime+mr.calcDuration(gzSpoil);
lTime2=(rf_180_duration+rf_180_duration)/2+system.rfRingdownTime+mr.calcDuration(gxSpoil);
lTime3=(rf_180_duration+rf_180_duration)/2+system.rfRingdownTime+mr.calcDuration(gxSpoil_2);

lTime4=TE/2-lTime2;
lTime5=TE/2-lTime1-lTime3;

%Define ADC events
adc = mr.makeAdc(Nx, 'Dwell', 2e-4, 'system', system);
delayTE1=lTime4-(rf_180_duration+gz.fallTime+mr.calcDuration(gySpoil_semiFinal));
delayTE2=lTime5-(rf_180_duration/2+mr.calcDuration(gySpoil_Final)-gySpoil_Final.delay)-adc.dwell/2;
adc.delay=delayTE2;

% Loop over repetitions and define sequence blocks
for i=1:Nrep
    seq.addBlock(rf_90, gx_p1);
    seq.addBlock(gzSpoil,gx_parts(2),rf_180_1,gy_p1);
    seq.addBlock(gxSpoil,gzSpoil_2,rf_180_2, gy_comb_parts(1));
    seq.addBlock(gxSpoil_2,gy_comb_parts(2),rf_180_3, gz);
    seq.addBlock(mr.makeDelay(delayTE1));
    seq.addBlock(gxSpoil_combi,gySpoil_combi,gz_comb,rf_180_4);
    seq.addBlock(adc,mr.makeDelay(mr.calcDuration(adc)+system.adcDeadTime));

    % this is realy a lazy way of defining the TR delay
    if i==1
        delayTR = TR- seq.duration();
        assert(delayTR>0);
    end

    seq.addBlock(mr.makeDelay(delayTR))
end

%% check whether the timing of the sequence is correct
[ok, error_report]=seq.checkTiming;

if (ok)
    fprintf('Timing check passed successfully\n');
else
    fprintf('Timing check failed! Error listing follows:\n');
    fprintf([error_report{:}]);
    fprintf('\n');
end
seq.setDefinition('FOV', [sliceThickness sliceThickness sliceThickness]);
seq.setDefinition('Name', 'semiLaser');

seq.write('semiLASER.seq');       % Write to pulseq file

%% do some visualizations

seq.plot('timeDisp','us','showBlocks',1,'timeRange',[0 TE*1.2]);
%seq.plot();             % Plot sequence waveforms

%% trajectory calculation
% [ktraj_adc, ktraj, t_excitation, t_refocusing, t_adc] = seq.calculateKspace();
% t_ktraj=(1:(size(ktraj,2)))*system.gradRasterTime;
[ktraj_adc, t_adc, ktraj, t_ktraj, t_excitation, t_refocusing] = seq.calculateKspacePP();
[ktraj_adc_ofs, t_adc_ofs, ktraj_ofs, t_ktraj_ofs] = seq.calculateKspacePP('gradient_offset',[1.0 .5 -1.0]*1e3); % this will help us to verify the correct echo positions

% plot "k-spaces" (gradient moments)
figure; plot(t_ktraj, ktraj'); % plot the entire k-space trajectory
hold on; plot(t_adc,ktraj_adc(1,:),'.'); % and sampling points on the kx-axis
plot(t_ktraj_ofs, ktraj_ofs');
axis([0 TE*2 -250 250]);
title('gradient moments without and with background gradients');

%% additional timing checks
% Karl Landheer et al define tau(1:5) and require tau_1+tau_2+tau_3 = tau_2+tau4
tau=diff([t_excitation t_refocusing t_adc(1)]);

if abs(sum(tau)-TE)>5e-5 % we tolerate an error of 1/2 grad rasters
    warning('TE calculation seems to be wrong, check timing!');
end

if abs(sum(tau([1 3 5]))-sum(tau([2 4])))>5e-5 % we tolerate an error of 1/2 grad rasters
    warning('spin echo condition is not fulfilled, check timing!');
end


function [TR, TT, ER, maxD, t] = icp(q,p,varargin)
% this is modified version of the original version
%
% Perform the Iterative Closest Point algorithm on three dimensional point
% clouds.
%
% [TR, TT] = icp(q,p)   returns the rotation matrix TR and translation
% vector TT that minimizes the distances from (TR * p + TT) to q.
% p is a 3xm matrix and q is a 3xn matrix.
%
% [TR, TT] = icp(q,p,k)   forces the algorithm to make k iterations
% exactly. The default is 10 iterations.
%
% [TR, TT, ER] = icp(q,p,k)   also returns the RMS of errors for k
% iterations in a (k+1)x1 vector. ER(0) is the initial error.
%
% [TR, TT, ER, t] = icp(q,p,k)   also returns the calculation times per
% iteration in a (k+1)x1 vector. t(0) is the time consumed for preprocessing.
%
% Additional settings may be provided in a parameter list:
%
% Boundary
%       {[]} | 1x? vector
%       If EdgeRejection is set, a vector can be provided that indexes into
%       q and specifies which points of q are on the boundary.
%
% EdgeRejection
%       {false} | true
%       If EdgeRejection is true, point matches to edge vertices of q are
%       ignored. Requires that boundary points of q are specified using
%       Boundary or that a triangulation matrix for q is provided.
%
% Extrapolation
%       {false} | true
%       If Extrapolation is true, the iteration direction will be evaluated
%       and extrapolated if possible using the method outlined by
%       Besl and McKay 1992.
%
% Matching
%       bruteForce | Delaunay | {kDtree}
%       Specifies how point matching should be done.
%       bruteForce is usually the slowest and kDtree is the fastest.
%       Note that the kDtree option is depends on the Statistics Toolbox
%       v. 7.3 or higher.
%
% Minimize
%       {point} | plane | lmaPoint
%       Defines whether point to point or point to plane minimization
%       should be performed. point is based on the SVD approach and is
%       usually the fastest. plane will often yield higher accuracy. It
%       uses linearized angles and requires surface normals for all points
%       in q. Calculation of surface normals requires substantial pre
%       proccessing.
%       The option lmaPoint does point to point minimization using the non
%       linear least squares Levenberg Marquardt algorithm. Results are
%       generally the same as in points, but computation time may differ.
%
% Normals
%       {[]} | n x 3 matrix
%       A matrix of normals for the n points in q might be provided.
%       Normals of q are used for point to plane minimization.
%       Else normals will be found through a PCA of the 4 nearest
%       neighbors.
%
% ReturnAll
%       {false} | true
%       Determines whether R and T should be returned for all iterations
%       or only for the last one. If this option is set to true, R will be
%       a 3x3x(k+1) matrix and T will be a 3x1x(k+1) matrix.
%
% Triangulation
%       {[]} | ? x 3 matrix
%       A triangulation matrix for the points in q can be provided,
%       enabling EdgeRejection. The elements should index into q, defining
%       point triples that act together as triangles.
%
% Verbose
%       {false} | true
%       Enables extrapolation output in the Command Window.
%
% Weight
%       {@(match)ones(1,m)} | Function handle
%       For point or plane minimization, a function handle to a weighting
%       function can be provided. The weighting function will be called
%       with one argument, a 1xm vector that specifies point pairs by
%       indexing into q. The weighting function should return a 1xm vector
%       of weights for every point pair.
%
% WorstRejection
%       {0} | scalar in ]0; 1[
%       Reject a given percentage of the worst point pairs, based on their
%       Euclidean distance.
%
% Martin Kjer and Jakob Wilm, Technical University of Denmark, 2012

% Use the inputParser class to validate input arguments.
inp = inputParser;

inp.addRequired('q', @(x)isreal(x) && size(x,1) == 3);
inp.addRequired('p', @(x)isreal(x) && size(x,1) == 3);

inp.addOptional('iter', 10, @(x)x > 0 && x < 10^5);

inp.addParamValue('Boundary', [], @(x)size(x,1) == 1);

inp.addParamValue('EdgeRejection', false, @(x)islogical(x));

inp.addParamValue('Extrapolation', false, @(x)islogical(x));

validMatching = {'bruteForce','Delaunay','kDtree'};
inp.addParamValue('Matching', 'kDtree', @(x)any(strcmpi(x,validMatching)));

validMinimize = {'point','plane','lmapoint'};
inp.addParamValue('Minimize', 'point', @(x)any(strcmpi(x,validMinimize)));

inp.addParamValue('Normals', [], @(x)isreal(x) && size(x,1) == 3);

inp.addParamValue('NormalsData', [], @(x)isreal(x) && size(x,1) == 3);

inp.addParamValue('ReturnAll', false, @(x)islogical(x));

inp.addParamValue('Triangulation', [], @(x)isreal(x) && size(x,2) == 3);

inp.addParamValue('Verbose', false, @(x)islogical(x));

inp.addParamValue('Weight', @(x)ones(1,length(x)), @(x)isa(x,'function_handle'));

inp.addParamValue('WorstRejection', 0, @(x)isscalar(x) && x > 0 && x < 1);

inp.addParamValue('SmartRejection', 0, @(x)isscalar(x) && x > 0);

inp.parse(q,p,varargin{:});
arg = inp.Results;
clear('inp');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Actual implementation

% Allocate vector for RMS of errors in every iteration.
t = zeros(arg.iter+1,1);

% Start timer
tic;

Np = size(p,2);

% Transformed data point cloud
pt = p;

% Allocate vector for RMS of errors in every iteration.
ER = zeros(arg.iter+1,1);
maxD = zeros(arg.iter,1);

% Initialize temporary transform vector and matrix.
T = zeros(3,1);
R = eye(3,3);

% Initialize total transform vector(s) and rotation matric(es).
TT = zeros(3,1, arg.iter+1);
TR = repmat(eye(3,3), [1,1, arg.iter+1]);

% If Minimize == 'plane', normals are needed
if (strcmp(arg.Minimize, 'plane') && isempty(arg.Normals))
    arg.Normals = lsqnormest(q,4);
end

% If Matching == 'Delaunay', a triangulation is needed
if strcmp(arg.Matching, 'Delaunay')
    DT = DelaunayTri(transpose(q));
end

% If Matching == 'kDtree', a kD tree should be built (req. Stat. TB >= 7.3)
if strcmp(arg.Matching, 'kDtree')
    kdOBJ = KDTreeSearcher(transpose(q));
end

% If edge vertices should be rejected, find edge vertices
if arg.EdgeRejection
    if isempty(arg.Boundary)
        bdr = find_bound(q, arg.Triangulation);
    else
        bdr = arg.Boundary;
    end
end

if arg.Extrapolation
    % Initialize total transform vector (quaternion ; translation vec.)
    qq = [ones(1,arg.iter+1);zeros(6,arg.iter+1)];
    % Allocate vector for direction change and change angle.
    dq = zeros(7,arg.iter+1);
    theta = zeros(1,arg.iter+1);
end

t(1) = toc;

% Go into main iteration loop
for k=1:arg.iter

    % Do matching
    switch arg.Matching
        case 'bruteForce'
            [match mindist] = match_bruteForce(q,pt);
        case 'Delaunay'
            [match mindist] = match_Delaunay(q,pt,DT);
        case 'kDtree'
            [match mindist] = match_kDtree(q,pt,kdOBJ);
    end

    % If matches to edge vertices should be rejected
    if arg.EdgeRejection
        p_idx = not(ismember(match, bdr));
        q_idx = match(p_idx);
        mindist = mindist(p_idx);
    else
        p_idx = true(1, Np);
        q_idx = match;
    end

    if k==1 && arg.SmartRejection
        arg.WorstRejection = sum(mindist > (median(mindist)* arg.SmartRejection))/ length(mindist);
        fprintf('ICP: Median=%f Threshold=%f WorstRejection=%f\n', median(mindist),median(mindist)* arg.SmartRejection,arg.WorstRejection);
        % vis
        %{
        figure
        hist(mindist,0:0.01/10:max(mindist)); hold on;
        plot(median(mindist),0,'g*');
        plot(median(mindist)* arg.SmartRejection,0,'r*');
        %}
    end

    % If worst matches should be rejected
    if arg.WorstRejection
        edge = round((1-arg.WorstRejection)*sum(p_idx));
        pairs = find(p_idx);
        [~, idx] = sort(mindist);
        p_idx(pairs(idx(edge:end))) = false;
        q_idx = match(p_idx);
        mindist = mindist(p_idx);
    end

    maxD(k) = max(mindist);

    if k == 1
        ER(k) = sqrt(sum(mindist.^2)/length(mindist));
    end

    switch arg.Minimize
        case 'point'
            % Determine weight vector
            weights = arg.Weight(match);
            [R,T] = eq_point(q(:,q_idx),pt(:,p_idx), weights(p_idx));
        case 'plane'
            weights = arg.Weight(match);
            [R,T] = eq_plane(q(:,q_idx),pt(:,p_idx),arg.Normals(:,q_idx),weights(p_idx));
        case 'lmaPoint'
            [R,T] = eq_lmaPoint(q(:,q_idx),pt(:,p_idx));
    end

    % Add to the total transformation
    TR(:,:,k+1) = R*TR(:,:,k);
    TT(:,:,k+1) = R*TT(:,:,k)+T;

    % Apply last transformation
    pt = TR(:,:,k+1) * p + repmat(TT(:,:,k+1), 1, Np);

    % Root mean of objective function
    ER(k+1) = rms_error(q(:,q_idx), pt(:,p_idx));

    % If Extrapolation, we might be able to move quicker
    if arg.Extrapolation
        qq(:,k+1) = [rmat2quat(TR(:,:,k+1));TT(:,:,k+1)];
        dq(:,k+1) = qq(:,k+1) - qq(:,k);
        theta(k+1) = (180/pi)*acos(dot(dq(:,k),dq(:,k+1))/(norm(dq(:,k))*norm(dq(:,k+1))));
        if arg.Verbose
            disp(['Direction change ' num2str(theta(k+1)) ' degree in iteration ' num2str(k)]);
        end
        if k>2 && theta(k+1) < 10 && theta(k) < 10
            d = [ER(k+1), ER(k), ER(k-1)];
            v = [0, -norm(dq(:,k+1)), -norm(dq(:,k))-norm(dq(:,k+1))];
            vmax = 25 * norm(dq(:,k+1));
            dv = extrapolate(v,d,vmax);
            if dv ~= 0
                q_mark = qq(:,k+1) + dv * dq(:,k+1)/norm(dq(:,k+1));
                q_mark(1:4) = q_mark(1:4)/norm(q_mark(1:4));
                qq(:,k+1) = q_mark;
                TR(:,:,k+1) = quat2rmat(qq(1:4,k+1));
                TT(:,:,k+1) = qq(5:7,k+1);
                % Reapply total transformation
                pt = TR(:,:,k+1) * p + repmat(TT(:,:,k+1), 1, Np);
                % Recalculate root mean of objective function
                % Note this is costly and only for fun!
                switch arg.Matching
                    case 'bruteForce'
                        [~, mindist] = match_bruteForce(q,pt);
                    case 'Delaunay'
                        [~, mindist] = match_Delaunay(q,pt,DT);
                    case 'kDtree'
                        [~, mindist] = match_kDtree(q,pt,kdOBJ);
                end
                ER(k+1) = sqrt(sum(mindist.^2)/length(mindist));
            end
        end
    end
    t(k+1) = toc;
end

if not(arg.ReturnAll)
    TR = TR(:,:,end);
    TT = TT(:,:,end);
end

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

function [match mindist] = match_bruteForce(q, p)
    m = size(p,2);
    n = size(q,2);
    match = zeros(1,m);
    mindist = zeros(1,m);
    for ki=1:m
        d=zeros(1,n);
        for ti=1:3
            d=d+(q(ti,:)-p(ti,ki)).^2;
        end
        [mindist(ki),match(ki)]=min(d);
    end

    mindist = sqrt(mindist);

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

function [match mindist] = match_Delaunay(q, p, DT)
	match = transpose(nearestNeighbor(DT, transpose(p)));
	mindist = sqrt(sum((p-q(:,match)).^2,1));

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

function [match mindist] = match_kDtree(~, p, kdOBJ)
	[match mindist] = knnsearch(kdOBJ,transpose(p));
    match = transpose(match);

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

function [R,T] = eq_point(q,p,weights)

m = size(p,2);
n = size(q,2);

% normalize weights
weights = weights ./ sum(weights);

% find data centroid and deviations from centroid
q_bar = q * transpose(weights);
q_mark = q - repmat(q_bar, 1, n);
% Apply weights
q_mark = q_mark .* repmat(weights, 3, 1);

% find data centroid and deviations from centroid
p_bar = p * transpose(weights);
p_mark = p - repmat(p_bar, 1, m);
% Apply weights
%p_mark = p_mark .* repmat(weights, 3, 1);

N = p_mark*transpose(q_mark); % taking points of q in matched order

[U,~,V] = svd(N); % singular value decomposition

R = V*diag([1 1 det(U*V')])*transpose(U);

T = q_bar - R*p_bar;

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

function [R,T] = eq_plane(q,p,n,weights)

n = n .* repmat(weights,3,1);

c = cross(p,n);

cn = vertcat(c,n);

C = cn*transpose(cn);

b = - [sum(sum((p-q).*repmat(cn(1,:),3,1).*n));
       sum(sum((p-q).*repmat(cn(2,:),3,1).*n));
       sum(sum((p-q).*repmat(cn(3,:),3,1).*n));
       sum(sum((p-q).*repmat(cn(4,:),3,1).*n));
       sum(sum((p-q).*repmat(cn(5,:),3,1).*n));
       sum(sum((p-q).*repmat(cn(6,:),3,1).*n))];

X = C\b;

cx = cos(X(1)); cy = cos(X(2)); cz = cos(X(3));
sx = sin(X(1)); sy = sin(X(2)); sz = sin(X(3));

R = [cy*cz cz*sx*sy-cx*sz cx*cz*sy+sx*sz;
     cy*sz cx*cz+sx*sy*sz cx*sy*sz-cz*sx;
     -sy cy*sx cx*cy];

T = X(4:6);

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

function [R,T] = eq_lmaPoint(q,p)

Rx = @(a)[1     0       0;
          0     cos(a)  -sin(a);
          0     sin(a)  cos(a)];

Ry = @(b)[cos(b)    0   sin(b);
          0         1   0;
          -sin(b)   0   cos(b)];

Rz = @(g)[cos(g)    -sin(g) 0;
          sin(g)    cos(g)  0;
          0         0       1];

Rot = @(x)Rx(x(1))*Ry(x(2))*Rz(x(3));

myfun = @(x,xdata)Rot(x(1:3))*xdata+repmat(x(4:6),1,length(xdata));

options = optimset('Algorithm', 'levenberg-marquardt');
x = lsqcurvefit(myfun, zeros(6,1), p, q, [], [], options);

R = Rot(x(1:3));
T = x(4:6);

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

% Extrapolation in quaternion space. Details are found in:
%
% Besl, P., & McKay, N. (1992). A method for registration of 3-D shapes.
% IEEE Transactions on pattern analysis and machine intelligence, 239?256.

function [dv] = extrapolate(v,d,vmax)

p1 = polyfit(v,d,1); % linear fit
p2 = polyfit(v,d,2); % parabolic fit
v1 = -p1(2)/p1(1); % linear zero crossing
v2 = -p2(2)/(2*p2(1)); % polynomial top point

if issorted([0 v2 v1 vmax]) || issorted([0 v2 vmax v1])
    disp('Parabolic update!');
    dv = v2;
elseif issorted([0 v1 v2 vmax]) || issorted([0 v1 vmax v2])...
        || (v2 < 0 && issorted([0 v1 vmax]))
    disp('Line based update!');
    dv = v1;
elseif v1 > vmax && v2 > vmax
    disp('Maximum update!');
    dv = vmax;
else
    disp('No extrapolation!');
    dv = 0;
end

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

% Determine the RMS error between two point equally sized point clouds with
% point correspondance.
% ER = rms_error(p1,p2) where p1 and p2 are 3xn matrices.

function ER = rms_error(p1,p2)
dsq = sum(power(p1 - p2, 2),1);
ER = sqrt(mean(dsq));

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

% Converts (orthogonal) rotation matrices R to (unit) quaternion
% representations
%
% Input: A 3x3xn matrix of rotation matrices
% Output: A 4xn matrix of n corresponding quaternions
%
% http://en.wikipedia.org/wiki/Rotation_matrix#Quaternion

function quaternion = rmat2quat(R)

Qxx = R(1,1,:);
Qxy = R(1,2,:);
Qxz = R(1,3,:);
Qyx = R(2,1,:);
Qyy = R(2,2,:);
Qyz = R(2,3,:);
Qzx = R(3,1,:);
Qzy = R(3,2,:);
Qzz = R(3,3,:);

w = 0.5 * sqrt(1+Qxx+Qyy+Qzz);
x = 0.5 * sign(Qzy-Qyz) .* sqrt(1+Qxx-Qyy-Qzz);
y = 0.5 * sign(Qxz-Qzx) .* sqrt(1-Qxx+Qyy-Qzz);
z = 0.5 * sign(Qyx-Qxy) .* sqrt(1-Qxx-Qyy+Qzz);

quaternion = reshape([w;x;y;z],4,[]);

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

% Converts (unit) quaternion representations to (orthogonal) rotation matrices R
%
% Input: A 4xn matrix of n quaternions
% Output: A 3x3xn matrix of corresponding rotation matrices
%
% http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#From_a_quaternion_to_an_orthogonal_matrix

function R = quat2rmat(quaternion)
q0(1,1,:) = quaternion(1,:);
qx(1,1,:) = quaternion(2,:);
qy(1,1,:) = quaternion(3,:);
qz(1,1,:) = quaternion(4,:);

R = [q0.^2+qx.^2-qy.^2-qz.^2 2*qx.*qy-2*q0.*qz 2*qx.*qz+2*q0.*qy;
     2*qx.*qy+2*q0.*qz q0.^2-qx.^2+qy.^2-qz.^2 2*qy.*qz-2*q0.*qx;
     2*qx.*qz-2*q0.*qy 2*qy.*qz+2*q0.*qx q0.^2-qx.^2-qy.^2+qz.^2];

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

% Least squares normal estimation from point clouds using PCA
%
% H. Hoppe, T. DeRose, T. Duchamp, J. McDonald, and W. Stuetzle.
% Surface reconstruction from unorganized points.
% In Proceedings of ACM Siggraph, pages 71:78, 1992.
%
% p should be a matrix containing the horizontally concatenated column
% vectors with points. k is a scalar indicating how many neighbors the
% normal estimation is based upon.
%
% Note that for large point sets, the function performs significantly
% faster if Statistics Toolbox >= v. 7.3 is installed.
%
% Jakob Wilm 2010

function n = lsqnormest(p, k)
m = size(p,2);
n = zeros(3,m);

v = ver('stats');
if str2double(v.Version) >= 7.5
    neighbors = transpose(knnsearch(transpose(p), transpose(p), 'k', k+1));
else
    neighbors = k_nearest_neighbors(p, p, k+1);
end

for i = 1:m
    x = p(:,neighbors(2:end, i));
    p_bar = 1/k * sum(x,2);

    P = (x - repmat(p_bar,1,k)) * transpose(x - repmat(p_bar,1,k)); %spd matrix P
    %P = 2*cov(x);

    [V,D] = eig(P);

    [~, idx] = min(diag(D)); % choses the smallest eigenvalue

    n(:,i) = V(:,idx);   % returns the corresponding eigenvector
end

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

% Program to find the k - nearest neighbors (kNN) within a set of points.
% Distance metric used: Euclidean distance
%
% Note that this function makes repetitive use of min(), which seems to be
% more efficient than sort() for k < 30.

function [neighborIds neighborDistances] = k_nearest_neighbors(dataMatrix, queryMatrix, k)

numDataPoints = size(dataMatrix,2);
numQueryPoints = size(queryMatrix,2);

neighborIds = zeros(k,numQueryPoints);
neighborDistances = zeros(k,numQueryPoints);

D = size(dataMatrix, 1); %dimensionality of points

for i=1:numQueryPoints
    d=zeros(1,numDataPoints);
    for t=1:D % this is to avoid slow repmat()
        d=d+(dataMatrix(t,:)-queryMatrix(t,i)).^2;
    end
    for j=1:k
        [s,t] = min(d);
        neighborIds(j,i)=t;
        neighborDistances(j,i)=sqrt(s);
        d(t) = NaN; % remove found number from d
    end
end

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

% Boundary point determination. Given a set of 3D points and a
% corresponding triangle representation, returns those point indices that
% define the border/edge of the surface.

function bound = find_bound(pts, poly)

%Correcting polygon indices and converting datatype
poly = double(poly);
pts = double(pts);

%Calculating freeboundary points:
TR = TriRep(poly, pts(1,:)', pts(2,:)', pts(3,:)');
FF = freeBoundary(TR);

%Output
bound = FF(:,1);

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


function stroud_test19 ( )

%*****************************************************************************80
%
%% TEST19 tests CUBE_UNIT_3D, QMULT_3D, RECTANGLE_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  global FUNC_3D_INDEX;

  a1 = -1.0;
  b1 = +1.0;

  a(1) = -1.0;
  a(2) = -1.0;
  a(3) = -1.0;
  b(1) = 1.0;
  b(2) = 1.0;
  b(3) = 1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST19\n' );
  fprintf ( 1, '  CUBE_UNIT_3D approximates integrals\n' );
  fprintf ( 1, '    in the unit cube in 3D.\n' );
  fprintf ( 1, '  QMULT_3D approximates triple integrals.\n' );
  fprintf ( 1, '  RECTANGLE_3D approximates integrals\n' );
  fprintf ( 1, '    in a rectangular block.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '    F(X)    CUBE_UNIT_3D  QMULT_3D      RECTANGLE_3D\n' );
  fprintf ( 1, '\n' );

  num = function_3d_num ( );

  for i = 1 : num

    FUNC_3D_INDEX = i;

    result1 = cube_unit_3d ( 'function_3d' );
    result2 = qmult_3d ( 'function_3d', a1, b1, 'fu18', 'fl18', 'fu28', 'fl28' );
    result3 = rectangle_3d ( 'function_3d', a, b );

    fname = function_3d_name ( i );

    fprintf ( 1, '  %s  %12f  %12f  %12f\n', ...
      fname, result1, result2, result3 );

  end

  return
end


function net = gp(nin, covar_fn, prior)
%GP	Create a Gaussian Process.
%
%	Description
%
%	NET = GP(NIN, COVARFN) takes the number of inputs NIN  for a Gaussian
%	Process model with a single output, together with a string COVARFN
%	which specifies the type of the covariance function, and returns a
%	data structure NET. The parameters are set to zero.
%
%	The fields in NET are
%	  type = 'gp'
%	  nin = number of inputs
%	  nout = number of outputs: always 1
%	  nwts = total number of weights and covariance function parameters
%	  bias = logarithm of constant offset in covariance function
%	  noise = logarithm of output noise variance
%	  inweights = logarithm of inverse length scale for each input
%	  covarfn = string describing the covariance function:
%	      'sqexp'
%	      'ratquad'
%	  fpar = covariance function specific parameters (1 for squared exponential,
%	   2 for rational quadratic)
%	  trin = training input data (initially empty)
%	  trtargets = training target data (initially empty)
%
%	NET = GP(NIN, COVARFN, PRIOR) sets a Gaussian prior on the parameters
%	of the model. PRIOR must contain the fields PR_MEAN and PR_VARIANCE.
%	If PR_MEAN is a scalar, then the Gaussian is assumed to be isotropic
%	and the additional fields NET.PR_MEAN and PR_VARIANCE are set.
%	Otherwise,  the Gaussian prior has a mean defined by a column vector
%	of parameters PRIOR.PR_MEAN and covariance defined by a column vector
%	of parameters PRIOR.PR_VARIANCE. Each element of PRMEAN corresponds
%	to a separate group of parameters, which need not be mutually
%	exclusive. The membership of the groups is defined by the matrix
%	PRIOR.INDEX in which the columns correspond to the elements of
%	PRMEAN. Each column has one element for each weight in the matrix, in
%	the order defined by the function GPPAK, and each element is 1 or 0
%	according to whether the parameter is a member of the corresponding
%	group or not.  The additional field NET.INDEX is set in this case.
%
%	See also
%	GPPAK, GPUNPAK, GPFWD, GPERR, GPCOVAR, GPGRAD
%

%	Copyright (c) Ian T Nabney (1996-2001)

net.type = 'gp';
net.nin = nin;
net.nout = 1;  % Only do single output GP

% Store log parameters
net.bias = 0;
net.min_noise = sqrt(eps);  % Prevent output noise collapsing completely
net.noise = 0;
net.inweights = zeros(1,nin);  % Weights on inputs in covariance function

covarfns = {'sqexp', 'ratquad'};

if sum(strcmp(covar_fn, covarfns)) == 0
  error('Undefined activation function. Exiting.');
else
  net.covar_fn = covar_fn;
end

switch covar_fn

  case 'sqexp'		% Squared exponential
    net.fpar = zeros(1,1);  % One function specific parameter

  case 'ratquad' 	% Rational quadratic
    net.fpar = zeros(1, 2); % Two function specific parameters

  otherwise
    error(['Unknown covariance function ', covar_fn]);
end

net.nwts = 2 + nin + length(net.fpar);

if nargin >= 3
  if size(prior.pr_mean) == [1 1]
    net.pr_mean = prior.pr_mean;
    net.pr_var = prior.pr_var;
  else
    net.pr_mean = prior.pr_mean;
    net.pr_var = prior.pr_var;
    net.index = prior.index;
  end
end

% Store training data as needed for gpfwd
net.tr_in = [];
net.tr_targets = [];


function [l, L_rf, L_sf, L_sk, L_hm, L_beta] = ...
    retroProjPlkLinFromPinHoleOnRob(Rf, Sf, Sk, hm, beta)

% RETROPROJPLKLINFROMPINHOLEONROB Retro-project Plucker line from pinhole on robot.
%
%   L = RETROPROJPLKLINFROMPINHOLEONROB(RF, SF, SK, HML, beta) gives the
%   retroprojected Plucker line L in World Frame from an observed
%   homogeneous line HM. RF and SF are Robot and Sensor Frames, SK is the
%   camera calibration parameters vector, HML is the detected homogeneous 2D
%   line and BETA is the non-measurable prior. L is a 6-vector :
%     L = [nx ny nz vx vy vz]'
%   with the 6 Plucker coordinates of a line in 3D space.
%
%   [L, L_rf, L_sf, L_k, L_hm, L_n] = ... returns the
%   Jacobians wrt RF.x, SF.x, SK, SC, HML and N.
%
%   See also INVPINHOLEIDP, FROMFRAMEIDP.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

% Frame World -> Robot  :  Rf
% Frame Robot -> Sensor :  Sf

if nargout == 1

    % L in Sensor Frame
    ls = invPinHolePlucker(Sk,hm,beta) ;

    % L in world frame
    lr = fromFramePlucker(Sf, ls) ;
    l  = fromFramePlucker(Rf, lr) ;

else

    % L in Sensor Frame
    [ls, LS_sk, LS_hm, LS_beta] = invPinHolePlucker(Sk,hm,beta) ;

    % L in world frame
    [lr, LR_sf, LR_ls] = fromFramePlucker(Sf, ls) ;
    [l,  L_rf,  L_lr]  = fromFramePlucker(Rf, lr) ;

    % chain rule for Jacobians
    L_sf   = L_lr*LR_sf ;
    L_ls   = L_lr*LR_ls ;
    L_sk   = L_ls*LS_sk ;
    L_hm   = L_ls*LS_hm ;
    L_beta = L_ls*LS_beta ;

end

return

%% test
syms rx ry rz ra rb rc rd sx sy sz sa sb sc sd u0 v0 au av h1 h2 h3 b1 b2 real

Rf.x = [rx;ry;rz;ra;rb;rc;rd];
Sf.x = [sx;sy;sz;sa;sb;sc;sd] ;

Rf   = updateFrame(Rf);
Sf   = updateFrame(Sf);
Sk   = [u0;v0;au;av];
hm   = [h1 h2 h3]';
beta = [b1 b2]';

%% test jacobian
[l,L_r,L_s,L_k,L_hm,L_beta] = retroProjPlkLinFromPinHoleOnRob(Rf,Sf,Sk,hm,beta)

%% down here tha Jac test - WARNING! IT TAKES AGES TO COMPUTE !!
simplify(L_r - jacobian(l,Rf.x))
simplify(L_s - jacobian(l,Sf.x))
% simplify(L_k - jacobian(l,Sk))
% simplify(L_hm - jacobian(l,hm))

%% numerical test
Rf.x = epose2qpose([0 0 0 deg2rad([0 0 0])]');
Sf.x = epose2qpose([0 0 0 deg2rad([90 0 90])]');
Rf   = updateFrame(Rf);
Sf   = updateFrame(Sf);
Sk   = [100 100 100 100]';
hm   = [1 0 0]';
beta = [1 0]';

[l,L_r,L_s,L_k,L_hm,L_beta]  = retroProjPlkLinFromPinHoleOnRob(Rf,Sf,Sk,hm,beta)
[l2,L2_s,L2_k,L2_hm,L2_beta] = retroProjectPlucker(Sf,Sk,hm,beta)

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function f = testFile(d1,d2,d3,d4,d5,d6,dth1,dth2,dth3,dth4,dth5,dth6,g,l1,l2,l3,l4,l5,m1,m2,m3,m4,m5,m6,th1,th2,th3,th4,th5,th6)
%TESTFILE
%    F = TESTFILE(D1,D2,D3,D4,D5,D6,DTH1,DTH2,DTH3,DTH4,DTH5,DTH6,G,L1,L2,L3,L4,L5,M1,M2,M3,M4,M5,M6,TH1,TH2,TH3,TH4,TH5,TH6)

%    This function was generated by the Symbolic Math Toolbox version 6.0.
%    04-Nov-2014 12:56:21

t2 = cos(th1);
t3 = dth2.^2;
t4 = th1-th2;
t5 = sin(t4);
t6 = dth3.^2;
t7 = th1-th3;
t8 = sin(t7);
t9 = dth4.^2;
t10 = th1-th4;
t11 = sin(t10);
t12 = dth5.^2;
t13 = th1-th5;
t14 = sin(t13);
t15 = cos(th2);
t16 = dth6.^2;
t17 = dth1.^2;
t18 = th2-th3;
t19 = sin(t18);
t20 = th2-th4;
t21 = sin(t20);
t22 = th2-th5;
t23 = sin(t22);
t24 = cos(th3);
t25 = th3-th4;
t26 = sin(t25);
t27 = th3-th5;
t28 = sin(t27);
t29 = cos(th4);
t30 = th4-th5;
t31 = sin(t30);
t32 = cos(th5);
t33 = th1-th6;
t34 = sin(t33);
t35 = th2-th6;
t36 = sin(t35);
t37 = th3-th6;
t38 = sin(t37);
t39 = th4-th6;
t40 = sin(t39);
t41 = th5-th6;
t42 = sin(t41);
f = [-d1.*g.*m1.*t2-g.*l1.*m2.*t2-g.*l1.*m3.*t2-g.*l1.*m4.*t2-g.*l1.*m5.*t2-g.*l1.*m6.*t2-d2.*l1.*m2.*t3.*t5-d3.*l1.*m3.*t6.*t8-d4.*l1.*m4.*t9.*t11-d5.*l1.*m5.*t12.*t14-d6.*l1.*m6.*t16.*t34-l1.*l2.*m3.*t3.*t5-l1.*l2.*m4.*t3.*t5-l1.*l2.*m5.*t3.*t5-l1.*l2.*m6.*t3.*t5-l1.*l3.*m4.*t6.*t8-l1.*l3.*m5.*t6.*t8-l1.*l3.*m6.*t6.*t8-l1.*l4.*m5.*t9.*t11-l1.*l4.*m6.*t9.*t11-l1.*l5.*m6.*t12.*t14;-d2.*g.*m2.*t15-g.*l2.*m3.*t15-g.*l2.*m4.*t15-g.*l2.*m5.*t15-g.*l2.*m6.*t15+d2.*l1.*m2.*t5.*t17-d3.*l2.*m3.*t6.*t19-d4.*l2.*m4.*t9.*t21-d5.*l2.*m5.*t12.*t23-d6.*l2.*m6.*t16.*t36+l1.*l2.*m3.*t5.*t17+l1.*l2.*m4.*t5.*t17+l1.*l2.*m5.*t5.*t17+l1.*l2.*m6.*t5.*t17-l2.*l3.*m4.*t6.*t19-l2.*l3.*m5.*t6.*t19-l2.*l3.*m6.*t6.*t19-l2.*l4.*m5.*t9.*t21-l2.*l4.*m6.*t9.*t21-l2.*l5.*m6.*t12.*t23;-d3.*g.*m3.*t24-g.*l3.*m4.*t24-g.*l3.*m5.*t24-g.*l3.*m6.*t24+d3.*l2.*m3.*t3.*t19+d3.*l1.*m3.*t8.*t17-d4.*l3.*m4.*t9.*t26-d5.*l3.*m5.*t12.*t28-d6.*l3.*m6.*t16.*t38+l2.*l3.*m4.*t3.*t19+l2.*l3.*m5.*t3.*t19+l1.*l3.*m4.*t8.*t17+l2.*l3.*m6.*t3.*t19+l1.*l3.*m5.*t8.*t17+l1.*l3.*m6.*t8.*t17-l3.*l4.*m5.*t9.*t26-l3.*l4.*m6.*t9.*t26-l3.*l5.*m6.*t12.*t28;-d4.*g.*m4.*t29-g.*l4.*m5.*t29-g.*l4.*m6.*t29+d4.*l2.*m4.*t3.*t21+d4.*l1.*m4.*t11.*t17+d4.*l3.*m4.*t6.*t26-d5.*l4.*m5.*t12.*t31-d6.*l4.*m6.*t16.*t40+l2.*l4.*m5.*t3.*t21+l2.*l4.*m6.*t3.*t21+l1.*l4.*m5.*t11.*t17+l1.*l4.*m6.*t11.*t17+l3.*l4.*m5.*t6.*t26+l3.*l4.*m6.*t6.*t26-l4.*l5.*m6.*t12.*t31;-d5.*g.*m5.*t32-g.*l5.*m6.*t32+d5.*l2.*m5.*t3.*t23+d5.*l1.*m5.*t14.*t17+d5.*l3.*m5.*t6.*t28+d5.*l4.*m5.*t9.*t31-d6.*l5.*m6.*t16.*t42+l2.*l5.*m6.*t3.*t23+l1.*l5.*m6.*t14.*t17+l3.*l5.*m6.*t6.*t28+l4.*l5.*m6.*t9.*t31;d6.*m6.*(-g.*cos(th6)+l2.*t3.*t36+l3.*t6.*t38+l1.*t17.*t34+l4.*t9.*t40+l5.*t12.*t42)];


function M = threed_bilinear( kernel, phi, test, w_g )
%-----------------------------------------------------------------------
%  threed_bilinear.m - routine to compute \int{ kernel*phi*test }
%                      (same as twod_bilinear)
%
%  Copyright (c) 2002, Jeff Borggaard, Virginia Tech
%  Version: 1.0
%
%  Usage:    M = threed_bilinear(kernel, phi, test, w_g)
%
%  Variables:     kernel
%                        Kernel function in the integral evaluated
%                        at the Gauss points
%
%                 phi
%                        matrix of element test functions evaluated
%                        at the Gauss points (dim: n_gauss, n_dof)
%
%                 test
%                        matrix of test functions evaluated at the
%                        Gauss points (dim: n_gauss, n_dof)
%
%                 w_g
%                        Row vector of Gauss weights
%-----------------------------------------------------------------------
%   [n_gauss,n_row] = size(test);
%   [n_g1   ,n_col] = size(phi );
%
%   M = zeros(n_row,n_col);
%   for i=1:n_row
%     for j=1:n_col
%        M(i,j) = ( w_g'    .* test(:,i)' ) * ( kernel .* phi(:,j) );
%     end
%   end

  M = test'*diag( w_g.*kernel )*phi;


function [SenErrDef, IniErrDef, ObsErrDef, ini_pva, wander, dt]=sys_path(dir_name,record)
rstat=[134 26 345 476 5456;654 7345 8345 9678 140];       %control how random the errors are generated
%rstat=rand(2,5)*30000;
dt=1/100;
D2R=pi/180;

%%%imu error parameters (discrete time param. defined at 100Hz)
%Accelerometers (defined in m-sec^2)
SenErrDef(1).A=1;
SenErrDef(1).B=3e-006;
SenErrDef(1).C=1;
SenErrDef(1).D=1e-3/sqrt(0.01);
SenErrDef(1).sP=sqrt(5.1e-007);
SenErrDef(1).tparam=[];

SenErrDef(2)=SenErrDef(1);
SenErrDef(3)=SenErrDef(1);

%Gyroscopes (defined in rad/sec)
SenErrDef(4).A=0.999980000199999;
SenErrDef(4).B=1.9999800001658e-006;
SenErrDef(4).C=1;
SenErrDef(4).D=0.0006/sqrt(0.01);
SenErrDef(4).sP=sqrt(1e-007);
SenErrDef(4).tparam=[];  %first param=sP of temperature scale factor (RC), second=B of temperature RW

SenErrDef(5)=SenErrDef(4);
SenErrDef(6)=SenErrDef(4);

% SenErrDef(6).A=1;
% SenErrDef(6).B=0;
% SenErrDef(6).C=1;
% SenErrDef(6).D=0;
% SenErrDef(6).sP=0;
% SenErrDef(1)=SenErrDef(6);
% SenErrDef(2)=SenErrDef(6);
% SenErrDef(3)=SenErrDef(6);
% SenErrDef(4)=SenErrDef(6);
% SenErrDef(5)=SenErrDef(6);

%%initial err defs
IniErrDef.pos_sP=diag([1/6e6,1/6e6,1]);
IniErrDef.vel_sP=eye(3)*0.01;
IniErrDef.att_sP=eye(3)*(10*pi/180);
IniErrDef.wander_sP=eye(2)*2;

%3D simulation in NED
%initial nav values
vel_b=zeros(3,1);
att_n=[-3;5;110]*D2R;
pos_n=[39.88864*D2R;32.78002*D2R;1102]; %51.081755-114.1360749

%%observation err defs
[Rn, Re, g, sL, cL, WIE_E]=geoparam_v000(pos_n);
Sn=Rn+pos_n(3);
Se=(Re+pos_n(3))*cos(pos_n(1));
ObsErrDef.sR=diag([0.5/Sn, 0.5/Se, 1.5]); %position error std (m)

%%%%%%%%%%% Generate path %%%%%%%%%%%%%%%%
if (record)
    %%motion definitions
    mot_def(1,:)=[1 0 0 0 0 5];
    mot_def(2,:)=[5 0 0 0 2 10];
    mot_def(3,:)=[1 0 0 0 0 5];
    mot_def(4,:)=[3 0 0 90*D2R 0 15];
    mot_def(5,:)=[1 0 0 0 0 5];
    mot_def(6,:)=[3 0 0 90*D2R 0 15];
    mot_def(7,:)=[1 0 0 0 0 5];
    mot_def(8,:)=[3 0 0 -90*D2R 0 15];
    mot_def(9,:)=[1 0 0 0 0 10];
    mot_def(10,:)=[3 0 0 -90*D2R 0 15];
    mot_def(11,:)=[1 0 0 0 0 5];
    mot_def(12,:)=[5 0 0 0 0 10];

    PathGen_v002(dir_name, [pos_n, vel_b, att_n], mot_def, [1 1/dt;1 0.1], 0, [], [], [], rstat(:,1));

    % %%%Add error
    % %Imu errors
    AddIMUErr_v000([dir_name 'mimu.bin'], [dir_name 'imu.bin'], [dir_name 'imuerr.bin'], 7, 2:7, SenErrDef, [], rstat(:,2));
    AddObsErr_v000([dir_name 'gps.bin'], 7, [dir_name 'obs_pos.bin'], [5 6 7], ObsErrDef, rstat(:,3));

    %AddIMUErr_v000([dir_name 'mimu.bin'], [dir_name 'imu.bin'], [dir_name 'imuerr.bin'], 7, 2:7, [], [], rstat(:,2));
    %AddObsErr_v000([dir_name 'gps.bin'], 7, [dir_name 'obs_pos.bin'], [5 6 7], [], rstat(:,3));
end

%initialization errors
randn('state',rstat(:,4));
pos_err=IniErrDef.pos_sP*randn(3,1);
vel_err=IniErrDef.vel_sP*randn(3,1);
att_err=IniErrDef.att_sP*randn(3,1);
Cerr=euler2dcm_v000(att_err);
Cbg=euler2dcm_v000(att_n);
Cbg_err=Cerr'*Cbg;
att_ini=dcm2euler_v000(Cbg_err);

ini_pva=[pos_n+pos_err vel_b+vel_err att_ini];  %%Attitude will be initialized using acc data
%wander=[sin(att_ini(3));cos(att_ini(3))];
wander=[sin(105*pi/180);cos(105*pi/180)];


%ISTEREO Stereo matching
%
% D = ISTEREO(LEFT, RIGHT, RANGE, H, OPTIONS) is a disparity image computed
% from the epipolar aligned stereo pair: the left image LEFT (HxW) and the
% right image RIGHT (HxW).  D (HxW) is the disparity and the value at each
% pixel is the horizontal shift of the corresponding pixel in IML as observed
% in IMR. That is, the disparity d=D(v,u) means that the pixel at RIGHT(v,u-d)
% is the same world point as the pixel at LEFT(v,u).
%
% RANGE is the disparity search range, which can be a scalar for disparities in
% the range 0 to RANGE, or a 2-vector [DMIN DMAX] for searches in the range
% DMIN to DMAX.
%
% H is the half size of the matching window, which can be a scalar for NxN or a
% 2-vector [N,M] for an NxM window.
%
% [D,SIM] = ISTEREO(LEFT, RIGHT, RANGE, H, OPTIONS) as above but returns SIM
% which is the same size as D and the elements are the peak matching score
% for the corresponding elements of D.  For the default matching metric ZNCC
% this varies between -1 (very bad) to +1 (perfect).
%
% [D,SIM,DSI] = ISTEREO(LEFT, RIGHT, RANGE, H, OPTIONS) as above but returns DSI
% which is the disparity space image (HxWxN) where N=DMAX-DMIN+1. The I'th
% plane is the similarity of IML to IMR shifted to the left by DMIN+I-1.
%
% [D,SIM,P] = ISTEREO(LEFT, RIGHT, RANGE, H, OPTIONS) if the 'interp' option is
% given then disparity is estimated to sub-pixel precision using quadratic
% interpolation.  In this case D is the interpolated disparity and P is
% a structure with elements A, B, dx.  The interpolation polynomial is
% s = Ad^2 + Bd + C where s is the similarity score and d is disparity relative
% to the integer disparity at which s is maximum.  P.A and P.B are matrices the
% same size as D whose elements are the per pixel values of the interpolation
% polynomial coefficients.  P.dx is the peak of the polynomial with respect
% to the integer disparity at which s is maximum (in the range -0.5 to +0.5).
%
% Options::
% 'metric',M   string that specifies the similarity metric to use which is
%              one of 'zncc' (default), 'ncc', 'ssd' or 'sad'.
% 'interp'     enable subpixel interpolation and D contains non-integer
%              values (default false)
% 'vshift',V   move the right image V pixels vertically with respect to left.
%
% Example::
%
% Load the left and right images
%         L = iread('rocks2-l.png', 'reduce', 2);
%         R = iread('rocks2-r.png', 'reduce', 2);
% then compute stereo disparity and display it
%         d = istereo(L, R, [40, 90], 3);
%         idisp(d);
%
% References::
%  - Robotics, Vision & Control, Section 14.3,
%    P. Corke, Springer 2011.
%
% Notes::
% - Images must be greyscale.
% - Disparity values pixels within a half-window dimension (H) of the edges
%   will not be valid and are set to NaN.
% - The C term of the interpolation polynomial is not computed or returned.
% - The A term is high where the disparity function has a sharp peak.
% - Disparity and similarity score can be obtained from the disparity space
%   image by [SIM,D] = max(DSI, [], 3)
%
%
% See also IRECTIFY, STDISP.

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB 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.
%
% MVTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.

function [disp,sim, o3] = istereo(L, R, drange, h, varargin)

% TODO: score cube is float, can return it

    opt.metric = 'zncc';
    opt.interp = false;
    opt.vshift = 0;

    opt = tb_optparse(opt, varargin);

    % ensure images are greyscale
    L = imono(L);
    R = imono(R);

    opt.vshift = round(opt.vshift);
    if opt.vshift ~= 0
        if opt.vshift > 0
            L = L(1:end-opt.vshift,:);
            R = R(opt.vshift:end,:);
        else
            vshift = -vshift;
            L = L(opt.vshift:end,:);
            R = R(1:end-opt.vshift,:);
        end
    end

    % compute the score cube, 3rd dimension is disparity
    DSI = stereo_match(L, R, 2*h+1, drange(1:2), opt.metric);

    % best value along disparity dimension is the peak
    %   s best score
    %   d disparity at which it occurs
    %
    % both s and d are matrices same size as L and R.
    if strcmp(opt.metric, 'sad') | strcmp(opt.metric, 'ssd')
        [s,d] = min(DSI, [], 3);
    else
        [s,d] = max(DSI, [], 3);
    end

    d(isnan(s)) = NaN;

    if opt.interp
        % interpolated result required

        % get number of pixels and disparity range
        npix = prod(size(L));

        if length(drange) == 1,
            ndisp = drange + 1;
        else
            dmin = min(drange);
            dmax = max(drange);
            ndisp = dmax - dmin + 1;
        end

        % find all disparities that are not at either end of the range, we need
        % a point on either side to interpolate them
        valid = (d>1) & (d<ndisp);
        valid = valid(:);

        % make a vector of consecutive pixel indices (1 to width*height)
        ci = [1:npix]';
        % turn disparities into a column vector
        dcol = d(:);

        % remove all entries that are not valid
        ci(~valid) = [];
        dcol(~valid) = [];

        % both ci and dcol have the same number of entries

        % for every valid pixel and disparity, find the index into the 3D score
        % array.  We cheat and consider that array WxHxD as a 2D array (WxH)xD
        %
        % We compute the indices for the best score and one each side of it
        k_m = sub2ind([npix ndisp], ci, dcol-1);
        k_0 = sub2ind([npix ndisp], ci, dcol);
        k_p = sub2ind([npix ndisp], ci, dcol+1);

        % initialize matrices (size of L and R) to hold the the best score
        % and the one each side of it
        y_m = ones(size(L))*NaN;
        y_0 = ones(size(L))*NaN;
        y_p = ones(size(L))*NaN;

        % now copy over the valid scores into these arrays.  What doesnt
        % get copies is a NaN
        y_m(ci) = DSI(k_m);
        y_0(ci) = DSI(k_0);
        y_p(ci) = DSI(k_p);

        % figure the coefficients of the peak fitting parabola:
        %    y = Ax^2 + Bx + C
        % Each coefficient is a matrix same size as (L and R)
        % We don't need to compute C
        A = 0.5*y_m - y_0 + 0.5*y_p;
        B = -0.5*y_m + 0.5*y_p;

        % now the position of the peak is given by -B/2A
        dx = -B ./ (2*A);

        % and we add this fractional part to the integer value obtained
        % from the max/min function
        d = d + dx;

   end
   d = d + drange(1)-1;

    if nargout > 0,
        disp = d;
    end
    if nargout > 1,
        sim = s;
    end

    if nargout > 2
        if opt.interp
            o3.A = A;
            o3.B = B;
            o3.dx = dx;
        else
            o3 = DSI;
        end
    end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_KUKA_KR6_2(robot, T)
%   Solves the inverse kinematic problem for the KUKA KR6_2 robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC__KUKA_KR6_2 returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('kuka', 'KR6_2');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%   Author: C.Escoto, E.Leon, V. Martinez & L. Mijares
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function q = inversekinematic_kuka_kr6_2(robot, T)

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

% %Evaluate the parameters
% theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
L6=abs(d(6));

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));
%q1=atan2(Py, Px);

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);
%the other possible solution is q1 + pi
q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);
%solver for q3 for both cases
q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solve_spherical_wrist(robot, q(:,i), T, 1,'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist(robot, q(:,i), T, -1, 'geometric'); %wrist down
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));
A2 = abs(a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%The inverse kinematic problem can be solved as in the IRB 140 (for example)

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2)));

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=abs(a(2));
L3=abs(d(4));

A2 = abs(a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%the angle phi is fixed
phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

eta = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) =  atan(L3/ A2)-eta;
q3(2) =  atan(L3/ A2)+ eta;


function [fBValue, fStdDev, fMc, fAValue, nNumberQuakes] = calc_BandMc(mCatalog, nMinimumNumber, mcCalcMethod, fBinning, bConstrainMc, fMcMin, fMcMax, fMcAdd)
%  b-values (with stddev) and the magnitudes of completeness of a given catalog
%
  % [fBValue, fStdDev, fMc, fAValue, nNumberQuakes] = calc_BandMc(mCatalog, nMinimumNumber, mcCalcMethod, fBinning, bConstrainMc, fMcMin, fMcMax, fMcAdd)
% --------------------------------------------------------------------------------------------------------------------------------------------------------------
% Calculation of the b-values, its standard deviation and the magnitudes of completeness of a given catalog
%
% Input parameters:
%   mCatalog            Earthquake catalog
%   nMinimumNumber      Minimum number of earthquakes in the catalog for calculating the output values
%   mcCalcMethod        Method to determine the magnitude of completeness (see also: McMethods)
%   fBinning            Magnitude binning of the catalog (default 0.1)
%   bConstrainMc        Constrain Mc to [fMcMin, fMcMax] if set to 1 (default 0)
%   fMcMin              see bConstrainMc
%   fMcMax              see bConstrainMc
%   fMcAdd              Value to be added to Mc after potential constraining and prior to
%                       b-value computation for conservative computations
%
% Output parameters:
%   fBValue             b-value of the catalog with respect to magnitude of completeness
%   fStdDev             Standard deviation of the b-value (Shi & Bolt)
%   fMc                 Magnitude of completeness (Mc)
%   fAValue            a-value of the catalog
%   nNumberQuakes       Number of quakes used to compute the b-value (M > Mc)
%
% Copyright (C) 2003 by Danijel Schorlemmer
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the
% Free Software Foundation, Inc.,
% 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.

report_this_filefun();

% Magnitude binning
if ~exist('fBinning', 'var')
  fBinning = 0.1;
end

% Constrain magnitude of completeness
if ~exist('bConstrainMc', 'var')
  bConstrainMc = false;
end
if ~exist('fMcMin', 'var')
  fMcMin = 0;
end
if ~exist('fMcMax', 'var')
  fMcMax = 1;
end

% Conservative adding of a constant
if ~exist('fMcAdd', 'var')
  fMcAdd = 0;
end

% Init output variables
fBValue = nan;
fStdDev = nan;
fMc = nan;
fAValue = nan;

try
  % Determine magnitude of completeness
  fMc = calc_Mc(mCatalog, mcCalcMethod, fBinning);
  % Constrain magnitude of completeness
  if bConstrainMc
    if fMc < fMcMin
      fMc = fMcMin;
    elseif fMc > fMcMax
      fMc = fMcMax;
    end
  end
  % Conservative Mc
  fMc = fMc + fMcAdd;
  % Calculate the b-value of the learning period
  vSel_ = mCatalog.Magnitude >= fMc;
  mCatalog = mCatalog.subset(vSel_);
  nNumberQuakes = mCatalog.Count;
  if nNumberQuakes >= nMinimumNumber
    [fBValue, fStdDev, fAValue] =  calc_bmemag(mCatalog.Magnitude, fBinning);
  end
  if isempty(fMc)
    fMc = nan;
  end
catch
end


function [fdMc, fProbability, fAICc] = calc_llhd_dMc2(mCat1, mCat2)
% function [fdMc, fProbability, fAICc] = calc_llhd_dMc2(mCat1, mCat2);
% ----------------------------------------------------------------------------------------------
% Calculate log-likelihood estimation of a magnitude of completness change between the two periods
%
% Incoming variable
% mCat1 : EQ catalog period 1 (Catalog to be modified)
% mCat2 : EQ catalog period 2 (Observed catalog)
%
% Outgoing variable
% fdMc         : Change in the magnitude of completeness
% fProbability : log-likelihood probabilty
% fAICc         : Corrected Akaike Information Criterion
%
% Author: J. Woessner, woessner@seismo.ifg,.ethz.ch
% last update: 21.10.03

fBinning = 0.1;
nSample = 100;
nMethod = 6;

% Determine exact time period
fPeriod1 = max(mCat1(:,3)) - min(mCat1(:,3));
fPeriod2 = max(mCat2(:,3)) - min(mCat2(:,3));

% Initialize values
fMinMag = min([min(mCat1(:,6)) min(mCat2(:,6))]);
fMaxMag = max([max(mCat1(:,6)) max(mCat2(:,6))]);

try
%% Calculate model for best fitting Mc, both periods
[mResult1, fMls1, fMc1, fMu1, fSigma1, mDataPredBest1, vPredBest1, fBValue1, fAvalue1] = calc_McCdfnormal(mCat1, fBinning);
[mResult, fMls, fMc2, fMu, fSigma, mDataPredBest, vPredBest, fBValue2, fAvalue2] = calc_McCdfnormal(mCat2, fBinning);

% [fMc1, fStd_Mc1, fBvalue1, fStd_B1, fAvalue1, fStd_A1, vMc1, mBvalue1] = calc_McBboot(mCat1, fBinning, nSample, nMethod)
% [fMc2, fStd_Mc2, fBvalue2, fStd_B2, fAvalue2, fStd_A2, vMc2, mBvalue2] = calc_McBboot(mCat2, fBinning, nSample, nMethod)

% Mc difference
fdMc = fMc2-fMc1;

% Select part of catalog
vSel = mCat1(:,6) >= fMc2-fBinning/2;
mCat1tmp=mCat1(vSel,:);

% Create Model distribution
vMagnitudes = [fMc2:0.1:floor(max(mCat2(:,6)))+0.1];
% Productuvity for fMc2
nNumberEvents = length(mCat1tmp(:,1));
vNumbers = 10.^(log10(nNumberEvents) - fBValue1*(vMagnitudes-fMc2));
%vNumbers = 10.^(fAvalue1- fBValue1*(vMagnitudes-fMc2));
vNumbers = round(vNumbers);
vNCumFMD = round(-diff(vNumbers));
% Calculate synthetic data below Mc
fMinMag = min(mCat2(:,6));
vMagstep = fMinMag:0.1:fMc2-0.1;
vProb = normcdf(vMagstep,fMu1, fSigma1);
vProb = vProb';
vMagstep = vMagstep';

% Calculate number of EQs in bins
fN_Mc = vNCumFMD(1,1);
vN = round(vProb(:,1)*fN_Mc);
%mNonCumModel = [vN vMagstep; vNCumFMD' vBin'];
mNonCumModel = [vN vMagstep; vNCumFMD' vMagnitudes(:,1:end-1)'];

% vMags = mDataPredBest(:,2)';
% FMD to be modeled (second period)
[vObsFMD,vBin2] = hist(mCat2(:,6),roundn(min(mCat2(:,6)),-1):0.1:floor(max(mCat2(:,6))));

% Select bins to calculate loglikelihood
vSel = (mNonCumModel(:,2) < min(vBin2) | mNonCumModel(:,2) > max(vBin2));
vPredFMD = mNonCumModel(~vSel,:);
% Normalize
vObsFMD = ceil(vObsFMD./fPeriod2);
vPredFMD(:,1) = vPredFMD(:,1)./fPeriod1;

if length(vObsFMD') ~= length(vPredFMD(:,1))
    disp('warning')
end
% Calculate the likelihood
vProb_ = calc_log10poisspdf2(vObsFMD', vPredFMD(:,1));
% Sum the probabilities
fProbability = (-1) * sum(vProb_);

nDegFree = 1; % degree of freedom
n_samples = length(mCat2(:,6));
%% Corrected Akaike Information Criterion (AICc)
fAICc = -2*(-fProbability)+2*nDegFree+2*nDegFree*(nDegFree+1)/(n_samples-nDegFree-1);

catch
    fdMc = nan;
    fProbability = nan;
    fAICc = nan;
end
% figure_w_normalized_uicontrolunits(30)
% %plot(mNonCumModel(:,2),mNonCumModel(:,1),'r*',vBin2,vObsFMD,'o')
% histogram(mCat1(:,6),fMinMag:0.1:fMaxMag);
% hold on
% histogram(mCat2(:,6),fMinMag:0.1:fMaxMag)
% h = findobj(gca,'Type','patch');
% set(h,'FaceColor','r','EdgeColor','w')
% plot(vPredFMD(:,2),vPredFMD(:,1),'g*',vBin2,vObsFMD,'o')


function perm=field3d(Nx,Ny,Nz,k_avg,V_dp,clx,cly,clz)
% Written by Ali A. Eftekhari
% See the license file
% Does not work! Needs lots of improvements
Lx=1.0; % domain length
x=linspace(-Lx/2.0,Lx/2.0,Nx);
y=linspace(-Lx/2.0,Lx/2.0,Ny);
z=linspace(-Lx/2.0,Lx/2.0,Nz);
[X,Y,Z]=ndgrid(x,y,z);
s=-log(1-V_dp); % standard deviation
mu=log(k_avg)-s*s/2.0; % mean for a log-random field
Z=s*randn(Nx,Ny,Nz); % normal distribution
F = exp(-(X.^2/(clx*clx/2.0)+Y.^2/(cly*cly/2.0)+Z.^2/(clz*clz/2.0)));
% Gaussian filter
f =2.0/sqrt(pi)*Lx/(Nx*Ny*Nz)^(1/3)/(clx*cly*clz)^(1/3).*ifftn(fft2(Z).*fftn(F)); % another filter
perm=exp(mu+real(f)); % perm field


function [medRV,minRV,medRVSS,minRVSS]=realized_min_med_variance(price,time,timeType,samplingType,samplingInterval,subsamples)
% Estimates truncated realized variance using the median and minimum method of Andersen, Dobrev and Schaumburg
%
% USAGE:
%   [MEDRV,MINRV] = realized_medrv(PRICE,TIME,TIMETYPE,SAMPLINGTYPE,SAMPLINGINTERVAL,SAMPLESPERBIN)
%   [MEDRV,MINRV,MEDRVSS,MINRVSS] = realized_medrv(PRICE,TIME,TIMETYPE,SAMPLINGTYPE,SAMPLINGINTERVAL,SAMPLESPERBIN,SUBSAMPLES)
%
% INPUTS:
%   PRICE            - m by 1 vector of high frequency prices
%   TIME             - m by 1 vector of times where TIME(i) corresponds to PRICE(i)
%   TIMETYPE         - String describing the way times are measured
%                       'wall'    24-hour clock of the form HHMMSS, e.g. 101543 or 153217
%                       'seconds' Time measured in seconds past midnight on the first day.
%                       'unit'  Unit normalized date format, e.g. .1, .234, .9
%                         Unit normalized times are more general than the other types and can be
%                         applied to data from more than one calendar day
%   SAMPLINGTYPE     - String describing the type of sampling to use when
%                        filtering PRICE
%                        'CalendarTime' - Sample in calendar time using observations separated by
%                          SAMPLINGINTERVAL seconds
%                        'CalendarUniform' - Sample in calendar time using SAMPLINGINTERVAL
%                          observations spread uniformly between TIME(1) and TIME(m)
%                        'BusinessTime' - Sample in business (tick) time using observation separated
%                          by SAMPLINGINTERVAL ticks
%                        'BusinessUniform' - Sample in business (tick) time using observations
%                          uniformly spaced in business time.
%                        'Fixed' - Sample at specific points in time. When using fixed,
%                          SAMPLINGINTERVAL must be a n by 1 vector of times with the same TIMETYPE
%                          as TIME (i.e. seconds if TIME is in seconds)
%   SAMPLINGINTERVAL  - Scalar integer or n by 1 vector whose meaning depends on SAMPLINGTYPE
%   SUBSAMPLES        - [OPTIONAL] Scalar integer indicating the number of subsample realized
%                         variance estimators to average with the original realized variance.
%                         Subsample realized variances are based on prices uniformly spaced between
%                         the times (Calendar sampling) or ticks (Business sampling).  SUBSAMPLES should
%                         be greater than 1, since 1 uses a single subsample and so is equivalent
%                         to the original RV will compute a subsample realized variance using the
%                         mid-point of the price sample points, 2 will use the original sample and
%                         the midpoint between the original sampling times, and so on.
%                         In general this number should be small so the subsample estimators will be "sparse".
%
% OUTPUTS:
%   MEDRV             - Median RV estimate
%   MINRV             - Minimum RV estimate
%   MEDRVSS           - Subsampled version of median RV estimate
%   MINRVSS           - Subsampled version of minimum RV estimate
%
% COMMENTS:
%
% EXAMPLE:
%  % Using all 1-minute returns between 9:30:00 and 16:00:00
%  fixedInterval = seconds2wall(wall2seconds(93000):60:wall2seconds(160000));
%  [MEDRV,MINRV] = realized_medrv(PRICE,TIME,'wall','Fixed',fixedInterval)
%
%  % Using all 5-minute returns between 9:30:00 and 16:00:00, but subsampling every minute
%  fixedInterval = seconds2wall(wall2seconds(93000):300:wall2seconds(160000));
%  [MEDRV,MINRV,MEDRVSS,MINRVSS] = realized_medrv(PRICE,TIME,'wall','Fixed',fixedInterval,5)
%
%  See also REALIZED_BIPOWER_VARIATION, REALIZED_KERNEL, REALIZED_QUANTILE_VARIANCE,
%  REALIZED_RANGE, REALIZED_PRICE_FILTER

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 10/24/2011

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin<5 || nargin>6
    error('Five or Six inputs required.')
end
if size(price,2)>size(price,1)
    price=price';
end
if size(price,2)>1
    error('PRICE must be a m by 1 vector.')
end
if size(time,2)>size(time,1)
    time=time';
end
if any(diff(time)<0)
    error('TIME must be sorted and increasing')
end
if size(time,2)>1 || length(time)~=length(price)
    error('TIME must be a m by 1 vector.')
end
% Inserted to protect against inputing integer times
time = double(time);

timeType=lower(timeType);
if ~ismember(timeType,{'wall','seconds','unit'})
    error('TIMETYPE must be one of ''wall'', ''seconds'' or ''unit''.');
end
samplingType=lower(samplingType);
if ~ismember(samplingType,{'calendartime','calendaruniform','businesstime','businessuniform','fixed'})
    error('SAMPLINGTYPE must be one of ''CalendarTime'', ''CalendarUniform'', ''BusinessTime'', ''BusinessUniform'' or ''Fixed''.');
end

m=size(price,1);
t0=time(1);
tT=time(m);
if ismember(samplingType,{'calendartime','calendaruniform','businesstime','businessuniform'})
    % Must be a scalar integer if timeType is seconds or wall
    if ismember(timeType,{'wall','seconds'})
        if ~isscalar(samplingInterval) || floor(samplingInterval)~=samplingInterval || samplingInterval<1
            error('SAMPLINGINTERVAL must be a positive integer for the SAMPLINGTYPE selected when using ''wall'' or ''seconds'' as TIMETYPE.')
        end
    else
        if ~isscalar(samplingInterval) || samplingInterval<0
            error('SAMPLINGINTERVAL must be a positive value for the SAMPLINGTYPE selected when using ''unit'' as TIMETYPE.')
        end
    end
else
    if size(samplingInterval,2)>size(samplingInterval,1)
        samplingInterval=samplingInterval';
    end
    if ~(any(samplingInterval>=t0) && any(samplingInterval<=tT))
        error('At least one sampling interval must be between min(TIME) and max(TIME) when using ''Fixed'' as SAMPLINGTYPE.')
    end
    if any(diff(samplingInterval)<=0)
        error('When using ''Fixed'' as SAMPLINGTYPE the vector of sampling times in SAMPLINGINTERVAL must be sorted and strictly increasing.')
    end
end

if nargin<6 || isempty(subsamples)
    subsamples = 1;
end

if nargin==6 && ~isempty(subsamples)
    if ~isscalar(subsamples) || subsamples<0 || floor(subsamples)~=subsamples
        error('SUBSAMPLES must be a non-negative scalar.')
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

logPrice = log(price);
% Convert times to unit if not already unit
if ~strcmp(timeType,'unit')
    [time,~,~,samplingInterval]=realized_convert2unit(time,timeType,samplingType,samplingInterval);
end
% Since everything is converted to unit, set timeType to unit
timeType='unit';
% m is the length of price

% Filter prices and compute the RV
filteredLogPrice = realized_price_filter(logPrice,time,timeType,samplingType,samplingInterval);
% Construct the aggregated return series.  Remember will need to skip
returns = diff(filteredLogPrice);
m = length(returns);

returns2 = (sqrt(m)*returns).^2;
r2 = [returns2(1:m-2) returns2(2:m-1) returns2(3:m)];
medRV = mean(median(r2,2));
medRVScale = pi/(6-4*sqrt(3)+pi);
medRV = medRV*medRVScale ;

r2 = [returns2(1:m-1) returns2(2:m)];
minRV = mean(min(r2,[],2));
minRVscale = pi/(pi-2);
minRV = minRV*minRVscale;

subsampledLogPrices = realized_subsample(logPrice,time,timeType,samplingType,samplingInterval,subsamples);
medRVs = nan(m*subsamples,1);
minRVs = nan(m*subsamples,1);
medRVcount = 0;
minRVcount = 0;
for i=1:subsamples
    filteredLogPrice = subsampledLogPrices{i};
    returns = diff(filteredLogPrice);
    m = size(returns,1);
    returns2 = m*returns.^2;
    % MedRV
    r2 = [returns2(1:m-2) returns2(2:m-1) returns2(3:m)];
    n = size(r2,1);
    medRVs(medRVcount+(1:n)) = median(r2,2);
    medRVcount = medRVcount  + n;
    % MinRV
    r2 = [returns2(1:m-1) returns2(2:m)];
    n = size(r2,1);
    minRVs(minRVcount+(1:n)) = min(r2,[],2);
    minRVcount = minRVcount + n;
end
medRVSS = mean(medRVs(1:medRVcount));
medRVSS = medRVSS * medRVScale;
minRVSS = mean(minRVs(1:minRVcount));
minRVSS = minRVSS * minRVscale;


function hintonDiagram(w,max_m,min_m)
% Display a matrix W where the square's area represents W(i,j)
% and red = negative, green = positive
%
%  Examples
%
%    W = randn(4,5); hintonDiagram(W)

% This file is from pmtk3.googlecode.com

% Based on hintonw function from Mathworks neural net toolbox

if nargin < 1,error('Not enough input arguments.');end
if nargin < 2, max_m = max(max(abs(w))); end
if nargin < 3, min_m = max_m / 100; end
if max_m == min_m, max_m = 1; min_m = 0; end

% DEFINE BOX EDGES
xn1 = [-1 -1 +1]*0.5;
xn2 = [+1 +1 -1]*0.5;
yn1 = [+1 -1 -1]*0.5;
yn2 = [-1 +1 +1]*0.5;

% DEFINE POSITIVE BOX
xn = [-1 -1 +1 +1 -1]*0.5;
yn = [-1 +1 +1 -1 -1]*0.5;

% DEFINE POSITIVE BOX
xp = [xn [-1 +1 +1 +0 +0]*0.5];
yp = [yn [+0 +0 +1 +1 -1]*0.5];

[S,R] = size(w);

cla reset
hold on
set(gca,'xlim',[0 R]+0.5);
set(gca,'ylim',[0 S]+0.5);
set(gca,'xlimmode','manual');
set(gca,'ylimmode','manual');
xticks = get(gca,'xtick');
set(gca,'xtick',xticks(find(xticks == floor(xticks))))
yticks = get(gca,'ytick');
set(gca,'ytick',yticks(find(yticks == floor(yticks))))
set(gca,'ydir','reverse');
if get(0,'screendepth') > 1
  %set(gca,'color',[1 1 1]*.5);
  %set(gcf,'color',[1 1 1]*.3);
end

for i=1:S
  for j=1:R
    m = sqrt((abs(w(i,j))-min_m)/max_m);
    m = min(m,max_m)*0.95;
    if real(m)
      if w(i,j) >= 0
        fill(xn*m+j,yn*m+i,[0 0.8 0])
        plot(xn1*m+j,yn1*m+i,'w',xn2*m+j,yn2*m+i,'k')
      elseif w(i,j) < 0
        fill(xn*m+j,yn*m+i,[0.8 0 0]);
        plot(xn1*m+j,yn1*m+i,'k',xn2*m+j,yn2*m+i,'w');
      end
    end
  end
end

plot([0 R R 0 0]+0.5,[0 0 S S 0]+0.5,'w');
%xlabel('Input');
%ylabel('Neuron');
grid on

end


function cy = zaxpy ( n, ca, cx, incx, cy, incy )

%*****************************************************************************80
%
%% ZAXPY computes a complex constant times a vector plus a vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for Fortran Usage,
%    Algorithm 539,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, September 1979, pages 308-323.
%
%  Parameters:
%
%    Input, integer N, the number of elements in CX and CY.
%
%    Input, complex CA, the multiplier of CX.
%
%    Input, complex CX(*), the first vector.
%
%    Input, integer INCX, the increment between successive entries of CX.
%
%    Input, complex CY(*), the second vector.
%
%    Input, integer INCY, the increment between successive entries of CY.
%
%    Output, complex CY(*), the value of CY(*) + CA * CX(*).
%
  cy(1:incy:1+(n-1)*incy) = cy(1:incy:1+(n-1)*incy) ...
    + ca * cx(1:incx:1+(n-1)*incx);

  return
end