function SO3VF = rdivide(SO3VF1, SO3VF2)
% overloads |SO3VF1 ./ SO3VF2|
%
% Syntax
%   SO3VF = SO3VF1 ./ SO3VF2
%   SO3VF = SO3VF1 ./ a
%   SO3VF = a ./ SO3VF2
%   SO3VF = SO3F .* SO3VF1
%   SO3VF = SO3VF1 .* SO3F
%
% Input
%  SO3VF1, SO3VF2 - @SO3VectorFieldHarmonic
%  a - double
%  SO3F - @SO3Fun
%
% Output
%  SO3VF - @SO3VectorFieldHarmonic
%

if isa(SO3VF2,'vector3d')
  SO3VF2 = SO3VF2.xyz.';
end

if isnumeric(SO3VF1)
  SO3VF = SO3VectorFieldHandle(@(rot) SO3VF1 ./ SO3VF2.eval(rot),SO3VF2.SRight,SO3VF2.SLeft);
  SO3VF = SO3VectorFieldHarmonic(SO3VF, 'bandwidth', min(getMTEXpref('maxSO3Bandwidth'),2*SO3VF2.bandwidth));
  return
end
if isnumeric(SO3VF2)
  SO3VF = times(SO3VF1,1./SO3VF2);
  return
end

ensureCompatibleSymmetries(SO3VF1,SO3VF2);
SO3VF = SO3VectorFieldHandle(@(rot) SO3VF1.eval(rot)./ SO3VF2.eval(rot),SO3VF1.SRight,SO3VF1.SLeft);
SO3VF = SO3VectorFieldHarmonic(SO3VF, 'bandwidth', min(getMTEXpref('maxSO3Bandwidth'),2*max(SO3VF1.bandwidth, SO3VF2.bandwidth)));

end


function [in3] = mi32in3(mi3)
% Convert volume from cubic miles to cubic inches.
% Chad Greene 2012
in3 = mi3*254358061050000;


%% Multiple comparison correction with Threshold-Free Cluster Enhancement
%
% This example demonstrates cosmo_cluster_neighborhood and
% cosmo_montecarlo_cluster_stat
%
% Note: this example shows multiple-comparison for a single subject, but
% the same logic can be applied to a group of subjects to do a group
% analysis.
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

%% Define data
config=cosmo_config();
data_path=fullfile(config.tutorial_data_path,'ak6','s01');

targets=repmat(1:6,1,10);
chunks=floor(((1:60)-1)/6)+1;

ds = cosmo_fmri_dataset(fullfile(data_path,'glm_T_stats_perrun.nii'),...
                        'mask',fullfile(data_path, 'brain_mask.nii'), ...
                                'targets',targets,'chunks',chunks);

% There are 10 chunks, for which the data is assumed to be independent.
% Construct a dataset with 10 samples corresponding to each chunk, with
% the average value across all six targets. Each sample is considered to be
% the same condition, namely the effect of the stimulus-versus-baseline
% effect; thus all target values must be set to 1.
%
% Use either:
%   - cosmo_split and cosmo_stack
%   - cosmo_average_samples
%   - (advanced) cosmo_fx
%
% Assign the result to a variable 'ds_stim'
% >@@>
ds_stim=cosmo_fx(ds,@(x)mean(x,1),{'chunks'});

% % alternative:
%     ds_stim=ds;
%     ds_stim.sa.targets(:)=1;
%     ds_split=cosmo_split(ds_stim,{'chunks'});
%
%     for k=1:numel(ds_split)
%         ds_avg_k=cosmo_slice(ds_split{k},1);
%         ds_avg_k.samples=mean(ds_split{k}.samples,1);
%         ds_split{k}=ds_avg_k;
%     end
%
%     ds_stim=cosmo_stack(ds_split);
%
% <@@<

%% Define a cluster neighborhood for this dataset and assign the result to
% a variable 'cl_nh'.
% hint: use cosmo_cluster_neighborhood

% >@@>
cl_nh=cosmo_cluster_neighborhood(ds_stim);
% <@@<

% Show a plot with the sorted number of neighbors
% for each voxel

% >@@>
n_neighbors_per_feature=cellfun(@numel,cl_nh.neighbors);
plot(sort(n_neighbors_per_feature))
% <@@<

%% Run cosmo_montecarlo_cluster_stat

% There is one condition per chunk; all targets are set to 1.
% Thus the subsequent anaylsis is a one-sample t-test.
% Note: if this was a group analysis, then each sample (row in ds.samples)
% would contain data from one subject; each unique value in .sa.chunks
% would correspond to one subject; and each unique value in .sa.targets
% would correspond to a condition of interest.

% Since this is a one-sample t-test against a mean of zero, we set this as
% a (required) option

opt=struct();
opt.h0_mean=0;

% set the number of iterations ('niter' option).
% At least 10000 is adviced for publication-quality analyses; because that
% takes quite a while to compute, here we use 200

% >@@>
% Note: for publication-quality analyses, niter=10000 or more is
% recommended
opt.niter=200;
% <@@<

% using cosmo_montecarlo_cluster_stat, compute a map with z-scores
% against the null hypothesis of a mean of zero, corrected for multiple
% comparisons. Store the result in a variable named 'tfce_z_ds_stim'

% >@@>
tfce_z_ds_stim=cosmo_montecarlo_cluster_stat(ds_stim,cl_nh,opt);
% <@@<
cosmo_plot_slices(tfce_z_ds_stim);

%% Using the same logic, run a two-sample t-test for primates versus bugs

% >@@>
primates_insects_mask=cosmo_match(ds.sa.targets,[1 2 5 6]);
ds_primates_insects=cosmo_slice(ds, primates_insects_mask);

% set primates=1, insects=2
ds_primates_insects.sa.targets(cosmo_match(...
                            ds_primates_insects.sa.targets,[1 2]))=1;
ds_primates_insects.sa.targets(cosmo_match(...
                            ds_primates_insects.sa.targets,[5 6]))=2;

% compute average for each unique combination of targets and chunks
ds_avg_primate_insects=cosmo_average_samples(ds_primates_insects);

cl_nh=cosmo_cluster_neighborhood(ds_avg_primate_insects);

opt=struct();

% set the number of iterations.
% At least 10000 is adviced for publication-quality analyses; because that
% takes quite a while to compute, here we use 200

% Note: for publication-quality analyses, niter=10000 or more is
% recommended
opt.niter=200;

tfce_z_ds_primate_vs_insects=cosmo_montecarlo_cluster_stat(...
                                    ds_avg_primate_insects,cl_nh,opt);
% <@@<
cosmo_plot_slices(tfce_z_ds_primate_vs_insects);


function [spectrum] = specest_nanfft(dat, time, varargin)

% SPECEST_NANFFT computes a fast Fourier transform in the presence of NaNs
% in the data
%
% Use as
%   [spectrum] = specest_nanfft(dat, ...)
% where
%   dat      = matrix of chan*sample
%   time     = vector, containing time in seconds for each sample
%   spectrum = matrix of taper*chan*foi*toi of fourier coefficients
%
% Optional arguments should be specified in key-value pairs and can include:
%   basis      = precomputes set of basis functions (sines/cosines)
%   datataype  = 0, 1, 2
%
% FIXME: FFT speed not yet optimized, e.g. MATLAB version, transpose or not, ...
% FIXME: function is recursive, should be avoided in favor of transparancy
%
% See also SPECEST_MTMFFT, SPECEST_CONVOL, SPECEST_HILBERT, SPECEST_MTMCONVOL, SPECEST_MVAR, SPECEST_WAVELET

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

% get the optional arguments
basis     = ft_getopt(varargin, 'basis');
datatype  = ft_getopt(varargin, 'datatype');

% datatype = 0, no missing data
% datatype = 1, the missing data is at the same location for all channels
% datatype = 2, the missing data is at different timepoints for different channels

% determine the data characteristics
[nchan, nsample] = size(dat);
fsample = 1./mean(diff(time));

if mod(nsample,2)==0
  % the number of samples is even
  k = nsample/2+1;
else
  % the number of samples is odd
  k = floor(nsample/2+1);
end

% determine the type of data and thereby the most suitable algorithm to use
nancount = sum(isnan(dat), 1);
if isempty(datatype)
  if all(nancount==0)
    % there is no missing data
    datatype = 0;
  elseif all(nancount==0 | nancount==nchan)
    % the missing data is at the same location for all channels
    datatype = 1;
  else
    % the missing data is at different timepoints for different channels
    datatype = 2;
  end
end

if datatype==0
  % no basis functions are needed, because the standard FFT routine will be used

elseif datatype~=0 && isempty(basis)
  % create a separate set of basis functions for the cosine and sine
  basis_c = zeros(k, nsample);
  basis_s = zeros(k, nsample);

  % create the time axis
  t = linspace(0, 2*pi, nsample+1);
  t = t(1:end-1);

  for w=1:k
    c = cos((w-1)*t);
    s = sin((w-1)*t);
    if w==1 || (w==(k) && mod(nsample,2)==0)
      % the normalization for the lowest (DC) and the highest frequency component is different
      s = s/(nsample);
      c = c/(nsample);
    else
      s = s/(nsample/2);
      c = c/(nsample/2);
    end
    basis_c(w,:) = c;
    basis_s(w,:) = s;
  end
  % concatenate the sine and cosine basis functions
  % leaving the first and last sine functions out, since those are all zero
  if mod(nsample,2)==0
    % the number of samples is even -> the last sine wave basis function is zero
    basis = cat(1, basis_c, basis_s(2:end-1, :));
  else
    % the number of samples is odd -> also include the last sine wave basis function
    basis = cat(1, basis_c, basis_s(2:end, :));
  end
end

switch datatype
  case 0
    % there is no missing data
    % use the standard FFT implementation
    y = fft(dat, [], 2);

  case 1
    % the missing data is at the same location for all channels
    % remove that piece from the data and from the basis functions and use linear estimation

    keep = ~isnan(dat(1,:));

    if all(~keep)
      % the data is all NaN, no reason to try to estimate the basis
      % functions
      y = nan(size(dat));

    else

      basis = basis(:,keep);
      dat = dat(:,keep);

      % do the linear estimation based on dat=y*basis
      % y = dat / basis;
      y = dat * pinv(basis);

      % disentagle the estimated components

      if mod(nsample,2)==0
        % the number of samples is even -> the last sine wave basis function is zero
        sel1 = 1;       % lowest cosine, i.e. DC
        sel2 = 2:(k-1); % all cosines in between
        sel3 = k;       % highest cosine
        sel4 = (k+1):nsample; % all sines

        est1 = y(:,sel1);
        est2 = y(:,sel2);
        est3 = y(:,sel3);
        est4 = y(:,sel4);

        % combine the various estimates into a complex representation compatible with standard FFT
        y_real = cat(2, est1,        est2, est3,          fliplr(est2));
        y_imag = cat(2, zeros(nchan,1), -est4, zeros(nchan,1),    fliplr(est4));
        y = y_real + i*y_imag;

      else
        % the number of samples is odd -> also include the last sine wave basis function
        sel1 = 1;       % lowest cosine, i.e. DC
        sel2 = 2:k;     % all other cosines
        sel3 = (k+1):nsample; % all sines

        est1 = y(:,sel1);
        est2 = y(:,sel2);
        est3 = y(:,sel3);

        % combine the various estimates into a complex representation compatible with standard FFT
        y_real = cat(2, est1,        est2, fliplr(est2));
        y_imag = cat(2, zeros(nchan,1), -est3, fliplr(est3));
        y = y_real + i*y_imag;
      end

    end % if all(~keep)

  case 2
    % the missing data is at different timepoints for different channels
    % use recursion to compute the nanfft for each channel
    y = zeros(size(dat));
    for k=1:nchan
      y(k,:) = specest_nanfft(dat(k,:), time, 'basis', basis);
    end

  otherwise
    ft_error('unsupported configuration of NaNs in the data');
end

% set output
spectrum = y;


function  vemPoisson3(cube, pde, option, varargin)

maxIt = option.maxIt;

%% Initialize err
errL2 = zeros(maxIt,1);   errH1 = zeros(maxIt,1);
erruIuh = zeros(maxIt,1); errMax = zeros(maxIt,1);
errTime = zeros(maxIt,1); solverTime = zeros(maxIt,1);
assembleTime = zeros(maxIt,1); meshTime = zeros(maxIt,1);
itStep = zeros(maxIt,1);  stopErr = zeros(maxIt,1); flag = zeros(maxIt,1);
N = zeros(maxIt,1);
h = zeros(maxIt,1);
h(1) = option.h0;

% [node,elem, interfaceData] = interfacemesh3(cube, pde.phi, h(1));
% checkinterfacemesh3(node,elem,interfaceData)
% [face, face2elem] = getpolymesh(elem,interfaceData);
% clear elem interfaceData

%         [node, elem] = cubehexmesh(cube,h(k+1));
%         idx = (1:size(elem, 1))';
%         face =  [elem(:,[1,4,3,2]); elem(:,[6, 7, 8, 5]);...
%                  elem(:,[2,3,7,6]); elem(:,[1, 5, 8, 4]);...
%                  elem(:,[1,2,6,5]); elem(:,[4, 8, 7, 3])];
%         face2elem = [idx;idx;idx;idx;idx;idx];
%         clear elem;

[node, elem] = cubemesh(cube, h(1));
elem = fixorder3(node,elem);
idx = (1:size(elem,1))';
face = [elem(:,[2, 3, 4]);elem(:,[1, 4,3]);elem(:,[1,2,4]);elem(:,[1,3,2])];
face(:,4) = 0;
face2elem = [idx;idx;idx;idx];
clear elem

%% Vertual Element Method
for k = 1:maxIt
    N(k) = size(node,1);
    [u, info] = Poisson3VEM(node, face, face2elem, pde, option);
    errMax(k) = max(abs(u - pde.exactu(node)));
    if k < maxIt
          h(k+1) = h(k)/2;

%         [node, elem, interfaceData] = interfacemesh3(cube, pde.phi, h(k+1));
%         h(k+1)
%         checkinterfacemesh3(node,elem,interfaceData)
%         [face, face2elem] = getpolymesh(elem,interfaceData);
%         clear elem interfaceData

%         [node, elem] = cubehexmesh(cube,h(k+1));
%         idx = (1:size(elem, 1))';
%         face =  [elem(:,[1,4,3,2]); elem(:,[6, 7, 8, 5]);...
%                  elem(:,[2,3,7,6]); elem(:,[1, 5, 8, 4]);...
%                  elem(:,[1,2,6,5]); elem(:,[4, 8, 7, 3])];
%         face2elem = [idx;idx;idx;idx;idx;idx];
%         clear elem;

        [node, elem] = cubemesh(cube, h(k+1));
        elem = fixorder3(node,elem);
        idx = (1:size(elem,1))';
        face = [elem(:,[2, 3, 4]);elem(:,[1, 4, 3]);...
                elem(:,[1, 2, 4]);elem(:,[1, 3, 2])];
        face(:,4) = 0;
        face2elem = [idx;idx;idx;idx];
        clear elem
    end
end

set(gcf, 'Units', 'normal');
set(gcf, 'Position', [0.25, 0.25, 0.55, 0.4]);
showrate(1./h(1:k), errMax(1:k), 1, 'k-+', '||u-u_h||');

end


function [err,time,solver,eqn] = femStokesHdiv(node,elem,pde,bdFlag,option,varargin)
%% FEMPOISSON solve Poisson equation by various finite element methods
% Created by Ming Wang, at Nov., 2012.
%

%% Check input arguments
if ~exist('node','var') || ~exist('elem','var')
    [node,elem] = squaremesh([0,1,0,1],0.25);  % default mesh is a square
end
if ~exist('option','var'), option = []; end
if ~exist('pde','var')
   pde = StokesZulehnerdata;                   % default data
end
if ~exist('bdFlag','var')
    bdFlag = setboundary(node,elem,'Dirichlet');
end
% default elemType
if ~isfield(option,'elemType')
    option.elemType = 'RT0-P0';
end

%% Parameters
option = femoption(option);
elemType = option.elemType;
maxIt = option.maxIt;
maxN = option.maxN;
L0 = option.L0;
refType = option.refType;

%% Generate an initial mesh
for k = 1:L0
    if strcmp(refType,'red')
        [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
    elseif strcmp(refType,'bisect')
        [node,elem,bdFlag] = uniformbisect(node,elem,bdFlag);
    end
end

%% Initialize err
erruL2 = zeros(maxIt,1); erruIuhH1 = zeros(maxIt,1); erruInf = zeros(maxIt,1);
errpL2 = zeros(maxIt,1); errpIphL2 = zeros(maxIt,1); errpInf = zeros(maxIt,1);
errpL2re = zeros(maxIt,1);
errwL2 = zeros(maxIt,1); errwIwh = zeros(maxIt,1);
errTime = zeros(maxIt,1); solverTime = zeros(maxIt,1);
assembleTime = zeros(maxIt,1); meshTime = zeros(maxIt,1);
itStep = zeros(maxIt,1);  stopErr = zeros(maxIt,1); flag = zeros(maxIt,1);
N = zeros(maxIt,1);

%% Finite Element Method
for k = 1:maxIt
    % solve the equation
    switch elemType
        case 'RT0-P0'   % RT0-P0 mixed FEM
            [u,p,w,edge,eqn,info] = StokesRT0(node,elem,bdFlag,pde,option);
        case 'BDM1B-P0' % (BDM1+bubble)-P0 mixed FEM
            [u,p,w,edge,eqn,info] = StokesBDM1B(node,elem,bdFlag,pde,option);
    end
    % compute error
    tic;
    % ================== error for velocity ==================
    if isfield(pde,'exactu')
        switch elemType
            case 'RT0-P0'
                erruL2(k) = getL2errorRT0(node,elem,pde.exactu,u);
                % interpolation
                uI = faceinterpolate(pde.exactu,node,edge,'RT0');
                ufreeDof = eqn.ufreeDof;
                u0 = u(ufreeDof);
                uI0 = uI(ufreeDof);
                erruIuhH1(k) = sqrt((u0-uI0)'*eqn.A*(u0-uI0));
                erruInf(k) = max(abs(u0-uI0));
            case 'BDM1B-P0'
                erruL2(k) = getL2errorBDM1(node,elem,pde.exactu,u);
                % interpolation
                uI = u;
                uI(1:2*size(edge,1)) = faceinterpolate(pde.exactu,node,edge,'BDM1');
                ufreeDof = eqn.ufreeDof;
                u0 = u(ufreeDof);
                uI0 = uI(ufreeDof);
                erruIuhH1(k) = sqrt((u0-uI0)'*eqn.A*(u0-uI0));
                erruInf(k) = max(abs(u0-uI0));
        end
    end
    % ================== error for pressure ==================
    if isfield(pde,'exactp')
        area = simplexvolume(node,elem);
        errpL2(k) = getL2error(node,elem,pde.exactp,p);
        pI = Lagrangeinterpolate(pde.exactp,node,elem,'P0');
        rp = recoverP02P1(node,elem,p,'LA');
        % --------------------------
        errpL2(k) = getL2error(node,elem,pde.exactp,p);
        errpL2re(k) = getL2error(node,elem,pde.exactp,rp);
        errpIphL2(k) = sqrt(dot((pI-p).^2,area));
        errpInf(k) = max(abs(p-pI));
    end
    % ================== error for vorticity ==================
    if isfield(pde,'exactw')
        switch elemType
            case 'RT0-P0'
                errwL2(k) = getL2error(node,elem,pde.exactw,w);
                wI = Lagrangeinterpolate(pde.exactw,node,elem,'P1');
                errwIwh(k) = sqrt((w-wI)'*eqn.Mv*(w-wI));
            case 'BDM1B-P0'
                errwL2(k) = getL2error(node,elem,pde.exactw,w);
                wI = Lagrangeinterpolate(pde.exactw,node,elem,'P2',edge);
                errwIwh(k) = sqrt((w-wI)'*eqn.Mv*(w-wI));
        end
    end
    % ================== record ==================
    errTime(k) = toc;
    % record time
    solverTime(k) = info.solverTime;
    assembleTime(k) = info.assembleTime;
    % record solver information
    itStep(k) = info.itStep;
    stopErr(k) = info.stopErr;
    flag(k) = info.flag;
    % plot
    N(k) = size(node,1);
    if option.plotflag && N(k) < 3e3 % show mesh and solution for small size
       figure(1); showresult(node,elem,p); pause(0.1);
       figure(2);
       showsolutionRT(node,elem,u); pause(0.1);
    end
    if N(k) > maxN
        break;
    end
    % refine mesh
    tic;
    if strcmp(refType,'red')
        [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
    elseif strcmp(refType,'bisect')
        [node,elem,bdFlag] = uniformbisect(node,elem,bdFlag);
    end
    meshTime(k) = toc;
end

%% Plot convergence rates
if option.rateflag
    figure;
    set(gcf,'Units','normal');
    set(gcf,'Position',[0.25,0.25,0.80,0.40]);
    subplot(1,3,1)
    h = 1./sqrt(N(1:k));
    showrateh3(h,erruL2(1:k),2,'k-+','||u-u_h||',...
               h,erruIuhH1(1:k),2,'r-*','||u_I-u_h||_1',...
               h,erruInf(1:k),2,'b-*','||u_I-u_h||_{\infty}');
    subplot(1,3,2)
    showrateh4(h,errpL2(1:k),2,'k-+', '||p - p_h||',...
               h,errpL2re(1:k),2,'g-+','||p- p_h^r||',...
               h,errpIphL2(1:k),2,'r-+','||p_I - p_h||',...
               h,errpInf(1:k),2,'b-+','||p_I - p_h||_{\infty}');
    subplot(1,3,3)
    showrateh2(h,errwL2(1:k),2,'k-+','||w - w_h||',...
               h,errwIwh(1:k),2,'r-+','||w_I - w_h||');
end

% Output
err = struct('N',N,'uL2',erruL2(1:k),'uInf',erruInf,'uIuhH1',erruIuhH1(1:k),...
             'pL2',errpL2(1:k),'pIphL2',errpIphL2(1:k),'pInf',errpInf(1:k),'pL2re',errpL2re(1:k),...
             'wL2',errwL2(1:k),'wIwhL2',errwIwh(1:k));
time = struct('N',N,'err',errTime(1:k),'solver',solverTime(1:k), ...
              'assmble',assembleTime(1:k),'mesh',meshTime(1:k));
solver = struct('N',N(1:k),'itStep',itStep(1:k),'time',solverTime(1:k),...
                'stopErr',stopErr(1:k),'flag',flag(1:k));

%% Display error on screen
ts = zeros(k,3); ts = char(ts);
% error of u
fprintf('===========================================================\n');
display('   N   ||u_I-u_h||_1  ||u-u_h||   ||u_I-u_h||_{max}');
display([num2str(err.N) ts num2str(err.uIuhH1,'%0.5e') ts num2str(err.uL2,'%0.5e') ts num2str(err.uInf,'%0.5e')]);
% error of p
fprintf('===========================================================\n');
display('   N   ||p_I-p_h||   ||p-p_h||    ||p_I-p_h||_{max}      ||p_I - p^r_h||');
display([num2str(err.N) ts num2str(err.pIphL2,'%0.5e') ts num2str(err.pL2,'%0.5e') ts num2str(err.pInf,'%0.5e') ts num2str(err.pL2re,'%0.5e')]);
% error of w
fprintf('===========================================================\n');
display('   N   ||w_I-w_h||   ||w-w_h||');
display([num2str(err.N) ts num2str(err.wIwhL2,'%0.5e') ts num2str(err.wL2,'%0.5e')]);
fprintf('===========================================================\n');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_C8(robot, T)
%   Solves the inverse kinematic problem for the KUKA KR5 ARC 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_KR5_ARCHW returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('EPSON', 'x');
%   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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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_C8(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 = [-Py Px 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));

%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 up
    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
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 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);

beta = 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) = pi - phi - beta;
q3(2) = pi - phi + beta;


function [esTSNR,esHRNR]=WienerNoiseReduction(ns,fs,IS)

% [esTSNR,esHRNR]=WIENERNOISEREDUCTION(ns,fs,IS)
%
% Title  :      Wiener Noise Suppressor with TSNR & HRNR algorithms
%
% Description : Wiener filter based on tracking a priori SNR using Decision-Directed
%               method, proposed by Plapous et al 2006. The two-step noise reduction
%               (TSNR) technique removes the annoying reverberation effect while
%               maintaining the benefits of the decision-directed approach. However,
%               classic short-time noise reduction techniques, including TSNR, introduce
%               harmonic distortion in the enhanced speech. To overcome this problem, a
%               method called harmonic regeneration noise reduction (HRNR)is implemented
%               in order to refine the a priori SNR used to compute a spectral gain able
%               to preserve the speech harmonics.
%
%
% Reference :   Plapous, C.; Marro, C.; Scalart, P., "Improved Signal-to-Noise Ratio
%               Estimation for Speech Enhancement", IEEE Transactions on Audio, Speech,
%               and Language Processing, Vol. 14, Issue 6, pp. 2098 - 2108, Nov. 2006
%
% Input Parameters :
%   ns          Noisy speech
%   fs          Sampling frequency (in Hz)
%   IS          Initial Silence (or non-speech activity) Period (in number of samples)
%
% Output Parameters : enhanced speech
%   esTSNR      enhanced speech with the Two-Step Noise Reduction method
%   esHNRN      enhanced speech with the Harmonic Regeneration Noise Reduction method
%
%Author :       LIU Ming, 2008
%Modified :     SCALART Pascal october, 2008
%
%

%% ------- input noisy speech  --------

l = length(ns);
s=ns;

wl = fix(0.020*fs)    % window length is 20 ms
NFFT=2*wl             % FFT size is twice the window length
hanwin = hanning(wl);

if (nargin<3 | isstruct(IS))
    IS=10*wl;             %Initial Silence or Noise Only part in samples (= ten frames)
end
%% -------- compute noise statistics ----------

nsum = zeros(NFFT,1);

count = 0;
    for m = 0:IS-wl
     nwin = s(m+1:m+wl).*hanwin;
      nsum = nsum + abs(fft(nwin,NFFT)).^2;
     count = count + 1;
    end

d= (nsum)/count;

%% --------- main algorithm ---------------
SP = 0.25;      % Shift percentage is 50 % Overlap-Add method works good with this value
normFactor=1/SP;
overlap = fix((1-SP)*wl); % overlap between sucessive frames
offset = wl - overlap;
max_m = fix((l-NFFT)/offset);

zvector = zeros(NFFT,1);
oldmag = zeros(NFFT,1);
news = zeros(l,1);

phasea=zeros(NFFT,max_m);
xmaga=zeros(NFFT,max_m);
tsnra=zeros(NFFT,max_m);
newmags=zeros(NFFT,max_m);

alpha = 0.98;

%Iteration to remove noise

%% --------------- TSNR ---------------------
for m = 0:max_m
   begin = m*offset+1;
   iend = m*offset+wl;
   speech = ns(begin:iend);       %extract speech segment
   winy = hanwin.*speech;   %perform hanning window
   ffty = fft(winy,NFFT);          %perform fast fourier transform
   phasey = angle(ffty);         %extract phase
   phasea(:,m+1)=phasey;       %for HRNR use
   magy = abs(ffty);             %extract magnitude
   xmaga(:,m+1)= magy;           %for HRNR use
   postsnr = ((magy.^2) ./ d)-1 ;      %calculate a posteriori SNR
   postsnr=max(postsnr,0.1);  % limitation to prevent distorsion

   %calculate a priori SNR using decision directed approach
   eta = alpha * ( (oldmag.^2)./d ) + (1-alpha) * postsnr;
   newmag = (eta./(eta+1)).*  magy;

   %calculate TSNR
   tsnr = (newmag.^2) ./ d;
   Gtsnr = tsnr ./ (tsnr+1);         %gain of TSNR
   tsnra(:,m+1)=Gtsnr;
   %Gtsnr=max(Gtsnr,0.1);
   Gtsnr = gaincontrol(Gtsnr,NFFT/2);

      %for HRNR use
   newmag = Gtsnr .* magy;
   newmags(:,m+1) = newmag;     %for HRNR use
   ffty = newmag.*exp(i*phasey);
   oldmag = abs(newmag);
   news(begin:begin+NFFT-1) = news(begin:begin+NFFT-1) + real(ifft(ffty,NFFT))/normFactor;
end

esTSNR=news;
%% --------------- HRNR -----------------------

%non linearity
newharm= max(esTSNR,0);
news = zeros(l,1);
%
for m = 0:max_m
   begin = m*offset+1;
   iend = m*offset+wl;

   nharm = hanwin.*newharm(begin:iend);
   ffth = abs(fft(nharm,NFFT));          %perform fast fourier transform

   snrham= ( (tsnra(:,m+1)).*(abs(newmags(:,m+1)).^2) + (1-(tsnra(:,m+1))) .* (ffth.^2) ) ./d;

   newgain= (snrham./(snrham+1));
   %newgain=max(newgain,0.1);

   newgain=gaincontrol(newgain,NFFT/2);

   newmag = newgain .*  xmaga(:,m+1);

   ffty = newmag.*exp(i*phasea(:,m+1));

   news(begin:begin+NFFT-1) = news(begin:begin+NFFT-1) + real(ifft(ffty,NFFT))/normFactor;
end;
%Output
esHRNR=news;

figure;
[B,f,T] = specgram(ns,NFFT,fs,hanning(wl),wl-10);
imagesc(T,f,20*log10(abs(B)));axis xy;colorbar
title(['Spectrogram - noisy speech'])
xlabel('Time (sec)');ylabel('Frequency (Hz)');

figure;
[B,f,T] = specgram(esTSNR,NFFT,fs,hanning(wl),wl-10);
imagesc(T,f,20*log10(abs(B)));axis xy;colorbar
title(['Spectrogram - output speech TSNR'])
xlabel('Time (sec)');ylabel('Frequency (Hz)');

figure;
[B,f,T] = specgram(esHRNR,NFFT,fs,hanning(wl),wl-10);
imagesc(T,f,20*log10(abs(B)));axis xy;colorbar
title(['Spectrogram - output speech HRNR'])
xlabel('Time (sec)');ylabel('Frequency (Hz)');

function        NewGain=gaincontrol(Gain,ConstraintInLength)
%
%Title  : Additional Constraint on the impulse response
%         to ensure linear convolution property
%
%
%Description :
%
% 1- The time-duration of noisy speech frame is equal to L1 samples.
%
% 2- This frame is then converted in the frequency domain
%       by applying a short-time Fourier transform of size NFFT leading
%       to X(wk) k=0,...,NFFT-1 when NFFT is the FFT size.
%
% 3- The estimated noise reduction filter is G(wk) k=0,1,...,NFFT-1
%       leading to an equivalent impulse response g(n)=IFFT[G(wk)]
%       of length L2=NFFT
%
% 4- When applying the noise reduction filter G(wk) to the noisy
%       speech spectrum X(wk), the multiplication S(wk)=G(wk)X(wk) is
%       equivalent to a convolution in the time domain. So the
%       time-duration of the enhanced speech s(n) should be equal to
%       Ltot=L1+L2-1.
%
% 5- If the length Ltot is greater than the time-duration of the IFFT[S(wk)]
%       the a time-aliasing effect will appear.
%
% 6- To overcome this phenomenon, the time-duration L2 of the equivalent
%       impulse response g(n) should be chosen such that Ltot = L1 + L2 -1 <= NFFT
%       => L2 <= NFFT+1-Ll
%
%       here we have NFFT=2*Ll so we should have L2 <= Ll+1. I have made
%       the following choice : the time-duration of g(n) is limited to
%       L2=NFFT/2=L1 (see lines 88 and 192)
%
%Author : SCALART Pascal
%
%October  2008
%

meanGain=mean(Gain.^2);
NFFT=length(Gain);

L2=ConstraintInLength;

win=hamming(L2);

% Frequency -> Time
% computation of the non-constrained impulse response
ImpulseR=real(ifft(Gain));

% application of the constraint in the time domain
ImpulseR2=[ImpulseR(1:L2/2).*win(1+L2/2:L2);zeros(NFFT-L2,1);ImpulseR(NFFT-L2/2+1:NFFT).*win(1:L2/2)];

% Time -> Frequency
NewGain=abs(fft(ImpulseR2,NFFT));

meanNewGain=mean(NewGain.^2);

NewGain=NewGain*sqrt(meanGain/meanNewGain); % normalisation to keep the same energy (if white r.v.)


%%*******************************************************************
%% schurmat_qblk: compute schur matrix corresponding to SOCP blocks.
%%
%% HKM direction: output = schur + Ax*Ae' + Ae*Ax' - Ad*Ad'
%% NT  direction: output = schur + Ae*Ae' - Ad*Ad'
%%
%% where schur = A*D*A', and Ad is the modification to ADA'
%% so that the latter is positive definite.
%%
%% [schur,UU,EE] = schurmat_qblk(blk,At,schur,UU,EE,p,dd,ee,xx);
%%
%% UU: stores the dense columns of Ax, Ae, Ad, and possibly
%%     those of A*D^{1/2}. It has the form UU = [Ax Ae Ad].
%% EE: stores the assocaited (2,2) block matrix when the
%%     output matrix is expressed as an augmented matrix.
%%     It has the form EE = [0 -lam 0; -lam 0 0; 0 0 I].
%%
%% options = 0, HKM
%%         = 1, NT
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

   function [schur,UU,EE] = schurmat_qblk(blk,At,par,schur,UU,EE,p,dd,ee,xx);

   global idxdenAq nnzschur_qblk

   if (nargin == 10); options = 0; else; options = 1; end;
   iter = par.iter;

   if isempty(EE)
      count = 0;
   else
      count = max(max(EE(:,2)),max(EE(:,1)));
   end
   pblk = blk(p,:); n = sum(pblk{2}); numblk = length(pblk{2});
%%
   Ae = qprod(pblk,At{p}',ee{p});
   if (options == 0)
      Ax = qprod(pblk,At{p}',xx{p});
   end
   idxden = checkdense(Ae);
   ddsch = dd{p};
   if ~isempty(idxden);
      spcolidx = setdiff([1:numblk],idxden);
      s = 1 + [0, cumsum(pblk{2})];
      idx = s(idxden);
      tmp = zeros(n,1);
      tmp(idx) = sqrt(2*abs(ddsch(idx)));
      Ad = qprod(pblk,At{p}',tmp);
      ddsch(idx) = abs(ddsch(idx));
      if (options == 0)
         len = length(idxden);
         gamzsub = par.gamz{p}(idxden);
         lam = gamzsub.*gamzsub;
         UU = [UU, Ax(:,idxden), Ae(:,idxden)*spdiags(lam,0,len,len), Ad(:,idxden)];
         tmp = count+[1:len]';
         EE = [EE; [tmp, len+tmp, -lam; len+tmp, tmp, -lam; ...
                    2*len+tmp, 2*len+tmp, ones(len,1)] ];
         count = count+3*len;
         Ax = Ax(:,spcolidx); Ae = Ae(:,spcolidx);
         tmp = Ax*Ae';
         schur = schur + (tmp + tmp');
      else
         len = length(idxden);
         w2 = par.gamz{p}./par.gamx{p};
         lam = w2(idxden);
         UU = [UU, Ae(:,idxden)*spdiags(sqrt(lam),0,len,len), Ad(:,idxden)];
         tmp = count+[1:len]';
         EE = [EE; [tmp, tmp, -lam; len+tmp, len+tmp, ones(len,1)] ];
         count = count + 2*len;
         Ae = Ae(:,spcolidx);
         schur = schur + Ae*Ae';
      end
   else
      if (options == 0)
         tmp = Ax*Ae';
         schur = schur + (tmp+tmp');
      else
         tmp = Ae*Ae';
         schur = schur + tmp;
      end
   end
   if (iter==1)
      idxdenAq{p} = checkdense(At{p}');
   end
   if ~isempty(idxdenAq{p});
      idxden = idxdenAq{p};
      len = length(idxden);
      Ad = At{p}(idxden,:)'*spdiags(sqrt(abs(ddsch(idxden))),0,len,len);
      UU = [UU, Ad];
      tmp = count+[1:len]';
      EE = [EE; [tmp, tmp, -sign(ddsch(idxden))]];
      count = count + len;
      ddsch(idxden) = zeros(len,1);
   end
   schurtmp = At{p}' *spdiags(ddsch,0,n,n) *At{p};
   schur = schur + schurtmp;
%%*******************************************************************


function gencost = modcost(gencost, alpha, modtype)
%MODCOST  Modifies generator costs by shifting or scaling (F or X).
%   NEWGENCOST = MODCOST(GENCOST, ALPHA)
%   NEWGENCOST = MODCOST(GENCOST, ALPHA, MODTYPE)
%
%   For each generator cost F(X) (for real or reactive power) in
%   GENCOST, this function modifies the cost by scaling or shifting
%   the function by ALPHA, depending on the value of MODTYPE, and
%   and returns the modified GENCOST. Rows of GENCOST can be a mix
%   of polynomial or piecewise linear costs. ALPHA can be a scalar,
%   applied to each row of GENCOST, or an NG x 1 vector, where each
%   element is applied to the corresponding row of GENCOST.
%
%   MODTYPE takes one of the 4 possible values (let F_alpha(X) denote the
%   the modified function):
%       'SCALE_F' (default) : F_alpha(X)         == F(X) * ALPHA
%       'SCALE_X'           : F_alpha(X * ALPHA) == F(X)
%       'SHIFT_F'           : F_alpha(X)         == F(X) + ALPHA
%       'SHIFT_X'           : F_alpha(X + ALPHA) == F(X)

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

%% define named indices into data matrices
[PW_LINEAR, POLYNOMIAL, MODEL, STARTUP, SHUTDOWN, NCOST, COST] = idx_cost;

if nargin < 3
    modtype = 'SCALE_F';
end

[ng, m] = size(gencost);

if ng ~= 0
    if length(alpha) ~= ng
        if length(alpha) == 1 && ng > 1     %% scalar, make it a col vector
            alpha = alpha * ones(ng, 1);
        else
            error('modcost: ALPHA must be a scalar or col vector with NG rows');
        end
    elseif size(alpha, 2) ~= 1
        alpha = alpha';                     %% convert row vector to col vector
    end

    ipwl = find(gencost(:, MODEL) == PW_LINEAR);
    ipol = find(gencost(:, MODEL) == POLYNOMIAL);
    npwl = length(ipwl);
    npol = length(ipol);
    c = gencost(ipol, COST:m);

    switch modtype
        case 'SCALE_F',
            if npol
                gencost(ipol, COST:m) = spdiags(alpha(ipol), 0, npol, npol) * c;
            end
            if npwl
                gencost(ipwl, COST+1:2:m) = spdiags(alpha(ipwl), 0, npwl, npwl) * ...
                    gencost(ipwl, COST+1:2:m);
            end
        case 'SCALE_X',
            for k = 1:length(ipol)
                n = gencost(ipol(k), NCOST);
                for i = 1:n
                    gencost(ipol(k), COST+i-1) = c(k, i) / alpha(ipol(k))^(n-i);
                end
            end
            if npwl
                gencost(ipwl, COST:2:m-1) = spdiags(alpha(ipwl), 0, npwl, npwl) * ...
                    gencost(ipwl, COST:2:m-1);
            end
        case 'SHIFT_F',
            for k = 1:length(ipol)
                n = gencost(ipol(k), NCOST);
                gencost(ipol(k), COST+n-1) = alpha(ipol(k)) + c(k, n);
            end
            if npwl
                gencost(ipwl, COST+1:2:m) = spdiags(alpha(ipwl), 0, npwl, npwl) * ...
                    ones(length(ipwl), (m+1-COST)/2) + gencost(ipwl, COST+1:2:m);
            end
        case 'SHIFT_X',
            for k = 1:length(ipol)
                n = gencost(ipol(k), NCOST);
                gencost(ipol(k), COST:COST+n-1) = polyshift(c(k, 1:n)', alpha(ipol(k)))';
            end
            if npwl
                gencost(ipwl, COST:2:m-1) = spdiags(alpha(ipwl), 0, npwl, npwl) * ...
                    ones(length(ipwl), (m+1-COST)/2) + gencost(ipwl, COST:2:m-1);
            end
        otherwise
            error('modcost: ''%s'' is not a valid modtype\n', modtype);
    end
end

%%-----  POLYSHIFT  -----
function d = polyshift(c, a)
%POLYSHIFT  Returns the coefficients of a horizontally shifted polynomial.
%
%   D = POLYSHIFT(C, A) shifts to the right by A, the polynomial whose
%   coefficients are given in the column vector C.
%
%   Example: For any polynomial with n coefficients in c, and any values
%   for x and shift a, the f - f0 should be zero.
%       x = rand;
%       a = rand;
%       c = rand(n, 1);
%       f0 = polyval(c, x)
%       f  = polyval(polyshift(c, a), x+a)

n = length(c);
d = zeros(size(c));
A = (-a * ones(n, 1)) .^ ((0:n-1)');
b = ones(n, 1);
for k = 1:n
    d(n-k+1) = b' * ( c(n-k+1:-1:1) .* A(1:n-k+1) );
    b = cumsum(b(1:n-k));
end


function [medianPrice,medianTime,totalVol,nObs] = realized_compute_median(price,time,volume)
% Computes the median price at each time stamp for a vector of prices which may have multiple
% observations with the same time stamp.
%
% USAGE:
%   [MEDIANPRICE,MEDIANTIME,TOTALVOL,NOBS] = realized_compute_median(PRICE,TIME,VOLUME)
%
% INPUTS:
%   PRICE            - m by 1 vector of high frequency prices
%   TIME             - m by 1 vector of times where TIME(i) corresponds to PRICE(i), sorted in
%                        ascending order
%   VOLUME           - [OPTIONAL] m by 1 vector of transaction volumes or bid/ask size.  If omitted
%                        TOTALVOLUME will be a vector of 0's.
%
% OUTPUTS:
%   MEDIANPRICE      - n by 1 vector of median prices where n is the number of unique elements of TIME
%   MEDIANTIME       - n by 1 vector of time stamps corresponding to MEDIANPRICE
%   TOTALVOLUME      - n by 1 vector capturing the total volume at each time stamp
%   NOBS             - n by 1 vector indicating number of prices at each element of MEDIANTIME
%
% COMMENTS:
%   This is a helper function for most of the Realized toolkit.  Most function in the Realized
%   toolkit expect time stamps to be unique.  Median price is a reasonable and fairly robust (to
%   noise) method of computing a unique price for each time stamp.

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 2    Date: 6/12/2011

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% InputChecking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin<2 || nargin>3
    error('Six or seven 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
m = size(price,1);

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)~=m
    error('TIME must be a m by 1 vector.')
end
time = double(time);

if nargin == 3
    if size(volume,2)>size(volume,1)
        volume=volume';
    end
    if size(volume,2)>1 || length(volume)~=m
        error('VOLUME must be a m by 1 vector.')
    end
else
    volume = zeros(m,1);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Make sure time and volume are double since I store it was uint32
time = double(time);
volume = double(volume);

% Quickly find the time change points
pl = find(diff(time));
pl = [1;pl+1;length(time)+1];

% Get the number of observations
N = length(pl);

% Pre-allocatte
medianPrice = zeros(N-1,1);
medianTime = time(pl(1:N-1));
totalVol = zeros(N-1,1);
nObs = zeros(N-1,1);

% Loop over unique sizes, with special cases for 1 and 2
[sizes,~,ind]=unique(diff(pl));
for i=1:length(sizes)
    j=find(ind==i);
    loc = bsxfun(@plus,pl(j),0:sizes(i)-1)';
    switch sizes(i)
        case 1
            medianPrice(j) = price(pl(j));
            totalVol(j) = volume(pl(j));
        case 2
            medianPrice(j) = (price(pl(j)) + price(pl(j)+1))/2;
        otherwise
            sortedPrice = sort(price(loc));
            if mod(sizes(i),2)==0
                medianPrice(j) = mean(sortedPrice([sizes(i)/2;sizes(i)/2+1],:))';
            else
                medianPrice(j) = sortedPrice(ceil(sizes(i)/2),:)';
            end
    end
    totalVol(j) = sum(volume(loc))';
    nObs(j) = sizes(i);
end

% Old, slower method
% Loop
% singleton = pldiff==1;
% medianPrice(singleton) = price(pl(singleton));
% totalVol(singleton) = volume(pl(singleton));
% nObs(singleton) = 1;
% notSingleton = find(~singleton);
%
% for i=1:length(notSingleton)
%     j = notSingleton(i);
%     medianPrice(j) = median(price(pl(j):pl(j+1)-1));
%     totalVol(j) = sum(volume(pl(j):pl(j+1)-1));
%     nObs(j) = pl(j+1)-pl(j)+1;
% end


function [fDeltaBValue, fBValueLearning, fBValueObserved] = kj_calcbvalues(mCatalog, fSplitTime, bLearningPeriod, fLearningPeriod, bObservedPeriod, fObservedPeriod, nMinimumNumber, nCalculateMC)
% function [fDeltaBValue, fBValueLearning, fBValueObserved]
%   = kj_calcbvalues(mCatalog, fSplitTime, bLearningPeriod, fLearningPeriod,
%                    bObservedPeriod, fObservedPeriod, nMinimumNumber, nCalculateMC)
% ----------------------------------------------------------------------------------
% Calculation of the b-values in the learning and observation (forecast) period
%
% Input parameters:
%   mCatalog            Earthquake catalog
%   fSplitTime          Time at which the catalog will be split
%   bLearningPeriod     Fix duration of the learning period (1 = fix, 0 = use catalog from start)
%   fLearningPeriod     Duration of the learning period
%   bObservedPeriod     Fix duration of the observation period (1 = fix, 0 = use catalog till end)
%   fObservedPeriod     Duration of the observation period
%   nMinimumNumber      Minimum number of earthquakes in the catalog for calculating the output values
%   nCalculateMC        Method to determine the magnitude of completeness (see also: help calc_Mc)
%
% Output parameters:
%   fDeltaBValue        Difference of both of the b-values (fBValueObserved - fBValueLearning)
%   fBValueLearning     b-value of the learning period
%   fBValueObserved     b-value of the observation (forecast) period
%
% Danijel Schorlemmer
% April 23, 2002

global bDebug;
if bDebug
  report_this_filefun(mfilename('fullpath'));
end

% Init output variables
fDeltaBValue = nan;
fBValueLearning = nan;
fBValueObserved = nan;

try
  % Create the catalogs for the learning and observation periods
  [mLearningCatalog, mObservedCatalog] = ex_SplitCatalog(mCatalog, fSplitTime, bLearningPeriod, fLearningPeriod, bObservedPeriod, fObservedPeriod);

  % Determine magnitude of completeness
  fMc = calc_Mc(mLearningCatalog, nCalculateMC);

  % Calculate the b-value of the learning period
  vSelection = mLearningCatalog(:,6) >= fMc;
  mLearningCatalog = mLearningCatalog(vSelection,:);
  if length(mLearningCatalog(:,1)) > nMinimumNumber
    [vDummy, fBValueLearning, vDummy, vDummy] =  bmemag(mLearningCatalog);
  end

  % Calculate the b-value of the observation period
  vSelection = mObservedCatalog(:,6) >= fMc;
  mObservedCatalog = mObservedCatalog(vSelection,:);
  if length(mObservedCatalog(:,1)) > nMinimumNumber
    [vDummy, fBValueObserved, vDummy, vDummy] =  bmemag(mObservedCatalog);
  end

  % Calculate the difference
  if (~isnan(fBValueLearning)) && (~isnan(fBValueObserved))
    fDeltaBValue = fBValueObserved - fBValueLearning;
  end
catch
end


% SP_EVALUATE_ELEMENT_LIST: compute the basis functions in a given list of elements.
%
%     sp = sp_evaluate_element_list (space, msh_elems, 'option1', value1, ...)
%
% INPUTS:
%
%    space:     object defining the space of discrete functions (see sp_vector)
%    msh_elems: msh structure containing the information of quadrature or
%               visualization points, for a given list of elements
%               (see msh_cartesian/msh_evaluate_element_list)
%   'option', value: additional optional parameters, currently available options are:
%
%              Name     |   Default value |  Meaning
%           ------------+-----------------+----------------------------------
%            value      |      true       |  compute shape_functions
%            gradient   |      false      |  compute shape_function_gradients
%            divergence |      false      |  compute shape_function_divs
%            curl       |      false      |  compute shape_function_curls
%
% OUTPUT:
%
%    sp: struct representing the discrete function space, with the following fields:
%              (see the article for a detailed description)
%
%    FIELD_NAME      (SIZE)                                     DESCRIPTION
%    ncomp           (scalar)                                   number of components of the functions of the space
%    ndof            (scalar)                                   total number of degrees of freedom
%    ndof_dir        (ncomp x ndim matrix)                      for each component, number of degrees of freedom along each direction
%    nsh_max         (scalar)                                   maximum number of shape functions per element
%    nsh             (1 x msh_col.nel vector)                   actual number of shape functions per each element
%    connectivity    (nsh_max x msh_col.nel vector)             indices of basis functions that do not vanish in each element
%    shape_functions (ncomp x msh_col.nqn x nsh_max x msh_col.nel)  basis functions evaluated at each quadrature node in each element
%    shape_function_gradients
%       (ncomp x rdim x msh_col.nqn x nsh_max x msh_col.nel)    basis function gradients evaluated at each quadrature node in each element
%    shape_function_divs (msh_col.nqn x nsh_max x msh_col.nel)  basis function divergence evaluated at each quadrature node in each element
%    shape_function_curls
%         2D:  (msh_col.nqn x nsh_max x msh_col.nel)            basis function curl evaluated at each quadrature node in each element
%         3D:  (3 x msh_col.nqn x nsh_max x msh_col.nel)
%
% Copyright (C) 2009, 2010, 2011 Carlo de Falco
% Copyright (C) 2011, 2015, 2019 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function sp = sp_evaluate_element_list (space, msh, varargin)

value = true;
gradient = false;
divergence = false;
curl = false;
hessian = false;
if (~isempty (varargin))
  if (~rem (length (varargin), 2) == 0)
    error ('sp_evaluate_element_list: options must be passed in the [option, value] format');
  end
  for ii=1:2:length(varargin)-1
    if (strcmpi (varargin {ii}, 'value'))
      value = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'gradient'))
      gradient = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'curl'))
      curl = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'divergence'))
      divergence = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'hessian'))
      hessian = varargin {ii+1};
    else
      warning ('Ignoring unknown option %s', varargin {ii});
    end
  end
end

grad_param = gradient || divergence || curl || hessian;
value_param = value || grad_param;
div_param = false; curl_param = false;
switch (lower (space.transform))
  case {'curl-preserving'}
    curl_param = curl;
  case {'div-preserving'}
    div_param = divergence;
end

sp = sp_evaluate_element_list_param (space, msh, 'value', value_param, 'gradient', grad_param, 'divergence', div_param, 'curl', curl_param, 'hessian', hessian);

if (isempty (msh.elem_list))
  return
end

switch (lower (space.transform))
  case {'grad-preserving'}
    sp = sp_vector_grad_preserving_transform (sp, msh, value, gradient, curl, divergence, hessian);
  case {'curl-preserving'}
    sp = sp_vector_curl_preserving_transform (sp, msh, value, curl);
    if (gradient || divergence || hessian)
      warning ('Gradient, divergence and hessian not implemented for curl-preserving transformation')
    end
  case {'div-preserving'}
    sp = sp_vector_div_preserving_transform (sp, msh, value, gradient, curl, divergence);
    if (hessian)
      warning ('Hessian not implemented for div-preserving transformation')
    end
end

end


function cS = antigorite
% serpentine

cs_Atg = crystalSymmetry('m',[4.7013 1 0.7844], [90, 91.633, 90]*degree);
N = Miller({0,0,1},{0,0,-1},{1,0,0},{-1,0,0},{2,1,0},{-2,1,0},cs_Atg);
dist = [0.15, 0.15, 0.4, 0.4, 2, 2];
cS = crystalShape(N./dist);


function HD = VARhd(VAR)
% =======================================================================
% Compute the historical decomposition of the time series in a VAR
% estimated with VARmodel and identified with VARir/VARfevd
% =======================================================================
% HD = VARhd(VAR)
% -----------------------------------------------------------------------
% INPUTS
%   - VAR: structure, result of VARmodel -> VARir/VARfevd function
% -----------------------------------------------------------------------
% OUTPUT
%   - HD: structure including the historical decomposition
% =======================================================================
% Ambrogio Cesa Bianchi, March 2015
% ambrogio.cesabianchi@gmail.com

% I thank Andrey Zubarev for finding a bug in the contribution of the
% exogenous variables when nvar_ex~=0 and nlag_ex>0.

%% Check inputs
%===============================================
if ~exist('VAR','var')
    error('You need to provide VAR structure, result of VARmodel');
end
% If there is VAR check that the inverse of the A matrix is not empty
invA = VAR.invA;
if isempty(invA)
    error('You need to identify the VAR before running VARhd. Run VARir/VARfevd first.');
end

%% Retrieve and initialize variables
%===============================================
Fcomp   = VAR.Fcomp;                      % Companion matrix
const   = VAR.const;                      % constant and/or trends
F       = VAR.Ft';                        % make comparable to notes
eps     = invA\transpose(VAR.residuals);  % structural errors
nvar    = VAR.nvar;                       % number of endogenous variables
nvar_ex = VAR.nvar_ex;                    % number of exogenous (excluding constant and trend)
nvarXeq = VAR.nvar * VAR.nlag;            % number of lagged endogenous per equation
nlag    = VAR.nlag;                       % number of lags
nlag_ex = VAR.nlag_ex;                    % number of lags of the exogenous
Y       = VAR.Y;                          % left-hand side
X       = VAR.X(:,1+const:nvarXeq+const); % right-hand side (no exogenous)
nobs    = size(Y,1);                      % number of observations

%% Compute historical decompositions
%===============================================

% Contribution of each shock
    invA_big = zeros(nvarXeq,nvar);
    invA_big(1:nvar,:) = invA;
    Icomp = [eye(nvar) zeros(nvar,(nlag-1)*nvar)];
    HDshock_big = zeros(nlag*nvar,nobs+1,nvar);
    HDshock = zeros(nvar,nobs+1,nvar);
    for j=1:nvar; % for each variable
        eps_big = zeros(nvar,nobs+1); % matrix of shocks conformable with companion
        eps_big(j,2:end) = eps(j,:);
        for i = 2:nobs+1
            HDshock_big(:,i,j) = invA_big*eps_big(:,i) + Fcomp*HDshock_big(:,i-1,j);
            HDshock(:,i,j) =  Icomp*HDshock_big(:,i,j);
        end
    end

% Initial value
    HDinit_big = zeros(nlag*nvar,nobs+1);
    HDinit = zeros(nvar, nobs+1);
    HDinit_big(:,1) = X(1,:)';
    HDinit(:,1) = Icomp*HDinit_big(:,1);
    for i = 2:nobs+1
        HDinit_big(:,i) = Fcomp*HDinit_big(:,i-1);
        HDinit(:,i) = Icomp *HDinit_big(:,i);
    end

% Constant
    HDconst_big = zeros(nlag*nvar,nobs+1);
    HDconst = zeros(nvar, nobs+1);
    CC = zeros(nlag*nvar,1);
    if const>0
        CC(1:nvar,:) = F(:,1);
        for i = 2:nobs+1
            HDconst_big(:,i) = CC + Fcomp*HDconst_big(:,i-1);
            HDconst(:,i) = Icomp * HDconst_big(:,i);
        end
    end

% Linear trend
    HDtrend_big = zeros(nlag*nvar,nobs+1);
    HDtrend = zeros(nvar, nobs+1);
    TT = zeros(nlag*nvar,1);
    if const>1;
        TT(1:nvar,:) = F(:,2);
        for i = 2:nobs+1
            HDtrend_big(:,i) = TT*(i-1) + Fcomp*HDtrend_big(:,i-1);
            HDtrend(:,i) = Icomp * HDtrend_big(:,i);
        end
    end

% Quadratic trend
    HDtrend2_big = zeros(nlag*nvar, nobs+1);
    HDtrend2 = zeros(nvar, nobs+1);
    TT2 = zeros(nlag*nvar,1);
    if const>2;
        TT2(1:nvar,:) = F(:,3);
        for i = 2:nobs+1
            HDtrend2_big(:,i) = TT2*((i-1)^2) + Fcomp*HDtrend2_big(:,i-1);
            HDtrend2(:,i) = Icomp * HDtrend2_big(:,i);
        end
    end

% Exogenous
    HDexo_big = zeros(nlag*nvar,nobs+1);
    HDexo = zeros(nvar,nobs+1);
    EXO = zeros(nlag*nvar,nvar_ex*(nlag_ex+1));
    if nvar_ex>0;
        VARexo = VAR.X_EX;
        EXO(1:nvar,:) = F(:,nvar*nlag+const+1:end); % this is c in my notes
        for i = 2:nobs+1
            HDexo_big(:,i) = EXO*VARexo(i-1,:)' + Fcomp*HDexo_big(:,i-1);
            HDexo(:,i) = Icomp * HDexo_big(:,i);
        end
    end

% All decompositions must add up to the original data
HDendo = HDinit + HDconst + HDtrend + HDtrend2 + HDexo + sum(HDshock,3);

%% Save and reshape all HDs
%===============================================
HD.shock = zeros(nobs+nlag,nvar,nvar);  % [nobs x shock x var]
    for i=1:nvar
        for j=1:nvar
            HD.shock(:,j,i) = [nan(nlag,1); HDshock(i,2:end,j)'];
        end
    end
HD.init   = [nan(nlag-1,nvar); HDinit(:,1:end)'];    % [nobs x var]
HD.const  = [nan(nlag,nvar);   HDconst(:,2:end)'];   % [nobs x var]
HD.trend  = [nan(nlag,nvar);   HDtrend(:,2:end)'];   % [nobs x var]
HD.trend2 = [nan(nlag,nvar);   HDtrend2(:,2:end)'];  % [nobs x var]
HD.exo    = [nan(nlag,nvar);   HDexo(:,2:end)'];     % [nobs x var]
HD.endo   = [nan(nlag,nvar);   HDendo(:,2:end)'];    % [nobs x var]