% this script tests GLCM features between CERR and pyradiomics on a wavelet filtered image.
%
% RKP, 03/22/2018

%% Load image
glcmParamFileName = fullfile(fileparts(fileparts(getCERRPath)),...
    'Unit_Testing','tests_for_cerr','test_glcm_radiomics_extraction_settings.json');
cerrFileName = fullfile(fileparts(fileparts(getCERRPath)),...
    'Unit_Testing','data_for_cerr_tests','CERR_plans','head_neck_ex1_20may03.mat.bz2');

planC = loadPlanC(cerrFileName,tempdir);
indexS = planC{end};

paramS = getRadiomicsParamTemplate(glcmParamFileName);
strNum = getMatchingIndex(paramS.structuresC{1},{planC{indexS.structures}.structureName});
scanNum = getStructureAssociatedScan(strNum,planC);

scanType = 'wavelet';
dirString = 'HHH';

%% Calculate features using CERR
harFeat3DdirS = calcGlobalRadiomicsFeatures...
            (scanNum, strNum, paramS, planC);
harlCombS = harFeat3DdirS.Wavelets_Coif1__HHH.glcmFeatS.AvgS;
cerrGlcmV = [harlCombS.autoCorr, harlCombS.jointAvg, harlCombS.clustPromin, harlCombS.clustShade, harlCombS.clustTendency, ...
harlCombS.contrast, harlCombS.corr, harlCombS.diffAvg, harlCombS.diffEntropy, harlCombS.diffVar, harlCombS.dissimilarity, ...
harlCombS.energy, harlCombS.jointEntropy, harlCombS.invDiff, harlCombS.invDiffMom, harlCombS.firstInfCorr, ...
harlCombS.secondInfCorr, harlCombS.invDiffMomNorm, harlCombS.invDiffNorm, harlCombS.invVar, ...
harlCombS.sumAvg, harlCombS.sumEntropy, harlCombS.sumVar];

% %% Calculate features using pyradiomics
% % image and mask for a structure
% testM = single(planC{indexS.scan}(scanNum).scanArray) - ...
%     single(planC{indexS.scan}(scanNum).scanInfo(1).CTOffset);
% mask3M = zeros(size(testM),'logical');
% [rasterSegments, planC, isError] = getRasterSegments(strNum,planC);
% [maskBoundBox3M, uniqueSlices] = rasterToMask(rasterSegments, scanNum, planC);
% mask3M(:,:,uniqueSlices) = maskBoundBox3M;
%
% dx = planC{indexS.scan}(scanNum).scanInfo(1).grid1Units;
% dy = planC{indexS.scan}(scanNum).scanInfo(1).grid1Units;
% dz = mode(diff([planC{indexS.scan}(scanNum).scanInfo(:).zValue]));
% pixelSize = [dx dy dz]*10;
%
% teststruct = PyradWrapper(testM, mask3M, pixelSize, scanType, dirString);
%
% %teststruct = PyradWrapper(testM, mask3M, scanType, dirString);
% pyradGlcmNamC = {'Autocorrelation', 'JointAverage', 'ClusterProminence', 'ClusterShade',  'ClusterTendency', ...
%     'Contrast', 'Correlation', 'DifferenceAverage', 'DifferenceEntropy', 'DifferenceVariance', 'Dissimilarity', ...
%     'JointEnergy', 'JointEntropy','Id','Idm', 'Imc1' , ...
%     'Imc2', 'Idmn','Idn','InverseVariance', 'sumAverage', 'SumEntropy', 'sumVariance'};
%
% pyradGlcmNamC = strcat(['wavelet','_', dirString,'_glcm_'],pyradGlcmNamC);
% pyRadGlcmV = [];
% for i = 1:length(pyradGlcmNamC)
%     if isfield(teststruct,pyradGlcmNamC{i})
%         pyRadGlcmV(i) = teststruct.(pyradGlcmNamC{i});
%     else
%         pyRadGlcmV(i) = NaN;
%     end
% end
%
% %% Compare
% glcmDiffV = (cerrGlcmV - pyRadGlcmV) ./ cerrGlcmV * 100

%% Compare using previously calculated values of pyradiomics glcm
saved_pyRadGlcmV = [929.516284841440,30.4950632056943,5316.83199378276,-0.303806680516285,20.9971321089971,22.7275310589439,-0.0380659828547803,2.78099962843313,3.04970358686672,14.7880425011987,NaN,0.0446949242970363,6.28336538646016,0.479852104826365,0.424210035556516,-0.0921069324860780,0.664903434331990,0.993049866792659,0.955511453133062,0.367836063144979,NaN,3.75942560575858,NaN];
glcmDiffV = (cerrGlcmV - saved_pyRadGlcmV) ./ cerrGlcmV * 100


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


%% Calculating optical flow of a sequence of images or from Video file
%% Author Gurkirt Singh
%% Date 09 June 2011 at Vision Lab IIT DELHI
%% Reading a Video file
clear all
clc
Object=mmreader('1.avi');
lastframe=read(Object, inf);
NumFrames = Object.NumberOfFrames;
%% Initialization of paramEters
%  Alpha
alpha=10;
% Number of iterations.
iterations=3;
% Do you Want to reduce size of Image for less computation? If yes DoYou=1;
DoYou=1;
% Width and Height
U=cell(NumFrames/2-1,1);
V=cell(NumFrames/2-1,1);
n=0;
% Normal Intensity flow

Normal=cell(NumFrames-1,1);

Window=[1/12 1/6 1/12;1/6 0 1/6;1/12 1/6 1/12];

%% Main program Starts here
hsize=[5 5];
sigma=1;
I=rgb2gray(read(Object,1));
h = fspecial('gaussian',hsize,sigma);
% I = imfilter(I,h,'replicate');
PreviousFrame= imfilter(I,h,'replicate');
if DoYou==1
PreviousFrame=impyramid(PreviousFrame,'reduce');
PreviousFrame=impyramid(PreviousFrame,'reduce');
end
[height width]=size(PreviousFrame);
%%
for k = 2:1:NumFrames

    I=rgb2gray(read(Object,k));
%     I=imfilter(I,h,'replicate');
    CurrentFrame=imfilter(I,h,'replicate');
    if DoYou==1;
    CurrentFrame=impyramid(CurrentFrame,'reduce');
    CurrentFrame=impyramid(CurrentFrame,'reduce');
    end

    [TempU TempV TempNormal]=Opticalflow(CurrentFrame,PreviousFrame,alpha,iterations,height,width);
    n=n+1;
    U{n,1}=single(TempU);
    V{n,1}=single(TempV);
%     Normal{n,1}=single(TempNormal);
    PreviousFrame=CurrentFrame;

end
%% Last Part
% myObj = VideoWriter('Ite3.wmv');
% myObj.FrameRate = 10;
% open(myObj);

% for i=1:49-1
% %     u=U{i,1};
% %     v=V{i,1};
% %     temp=mat2gray(sqrt(v.^2+u.^2));
% temp=mat2gray(abs(Normal{i,1}));
%     imshow(temp);
% %     BW =im2bw(I,0.12);
%     currFrame = getframe;
%     writeVideo(myObj,currFrame);
% end
% close(myObj)


classdef SymmetricTensorInverter < Inverter

    properties (Access = private)
        voigtTensor
        invVoigtTensor
    end

    methods (Access = public)

        function obj = SymmetricTensorInverter(tensor)
            obj.compute(tensor);
        end
    end

    methods (Access = protected)

        function computeInverse(obj)
            obj.transformTensor2Voigt()
            obj.makeInverseOfVoigtTensor()
            obj.transformVoigt2Tensor()
        end
    end

    methods (Access = private)

        function transformTensor2Voigt(obj)
            obj.voigtTensor = Tensor2VoigtConverter.convert(obj.tensor);
        end

        function makeInverseOfVoigtTensor(obj)
            obj.invVoigtTensor = Inverter.invert(obj.voigtTensor);
        end

        function transformVoigt2Tensor(obj)
            invT = obj.invVoigtTensor;
            obj.invertedTensor = Voigt2TensorConverter.convert(invT);
        end

    end

end


function [fhat,xhat,fcount,retcode] = sims_csminit(fcn,x0,f0,g0,badg,H0,varargin)
% [fhat,xhat,fcount,retcode] = csminit(fcn,x0,f0,g0,badg,H0,...
%                                       P1,P2,P3,P4,P5,P6,P7,P8)
% retcodes: 0, normal step.  5, largest step still improves too fast.
% 4,2 back and forth adjustment of stepsize didn't finish.  3, smallest
% stepsize still improves too slow.  6, no improvement found.  1, zero
% gradient.
%---------------------
% Modified 7/22/96 to omit variable-length P list, for efficiency and compilation.
% Places where the number of P's need to be altered or the code could be returned to
% its old form are marked with ARGLIST comments.
%
% Fixed 7/17/93 to use inverse-hessian instead of hessian itself in bfgs
% update.
%
% Fixed 7/19/93 to flip eigenvalues of H to get better performance when
% it's not psd.
%
%tailstr = ')';
%for i=nargin-6:-1:1
%   tailstr=[ ',P' num2str(i)  tailstr];
%end
%ANGLE = .03;
ANGLE = .005;
%THETA = .03;
THETA = .3; %(0<THETA<.5) THETA near .5 makes long line searches, possibly fewer iterations.
FCHANGE = 1000;
MINLAMB = 1e-9;
% fixed 7/15/94
% MINDX = .0001;
% MINDX = 1e-6;
MINDFAC = .01;
fcount=0;
lambda=1;
xhat=x0;
f=f0;
fhat=f0;
g = g0;
gnorm = norm(g);
%
if (gnorm < 1.e-12) & ~badg % put ~badg 8/4/94
   retcode =1;
   dxnorm=0;
   % gradient convergence
else
   % with badg true, we don't try to match rate of improvement to directional
   % derivative.  We're satisfied just to get some improvement in f.
   %
   %if(badg)
   %   dx = -g*FCHANGE/(gnorm*gnorm);
   %  dxnorm = norm(dx);
   %  if dxnorm > 1e12
   %     disp('Bad, small gradient problem.')
   %     dx = dx*FCHANGE/dxnorm;
   %   end
   %else
   % Gauss-Newton step;
   %---------- Start of 7/19/93 mod ---------------
   %[v d] = eig(H0);
   %toc
   %d=max(1e-10,abs(diag(d)));
   %d=abs(diag(d));
   %dx = -(v.*(ones(size(v,1),1)*d'))*(v'*g);
%      toc
   dx = -H0*g;
%      toc
   dxnorm = norm(dx);
   if dxnorm > 1e12
      dx = dx*FCHANGE/dxnorm;
   end
   dfhat = dx'*g0;
   %end
   %
   %
   if ~badg
      % test for alignment of dx with gradient and fix if necessary
      a = -dfhat/(gnorm*dxnorm);
      if a<ANGLE
         dx = dx - (ANGLE*dxnorm/gnorm+dfhat/(gnorm*gnorm))*g;
         % suggested alternate code:  ---------------------
         dx = dx*dxnorm/norm(dx)    % This keeps scale invariant to the angle correction
         % ------------------------------------------------
         dfhat = dx'*g;
         % dxnorm = norm(dx);  % this line unnecessary with modification that keeps scale invariant
      end
   end
   %
   % Have OK dx, now adjust length of step (lambda) until min and
   % max improvement rate criteria are met.
   done=0;
   factor=3;
   shrink=1;
   lambdaMin=0;
   lambdaMax=inf;
   lambdaPeak=0;
   fPeak=f0;
   lambdahat=0;
   while ~done
      if size(x0,2)>1
         dxtest=x0+dx'*lambda;
      else
         dxtest=x0+dx*lambda;
      end
      % home
      f = feval(fcn,dxtest,varargin{:});
      if f<fhat
         fhat=f;
         xhat=dxtest;
         lambdahat = lambda;
      end
      fcount=fcount+1;
      shrinkSignal = (~badg & (f0-f < max([-THETA*dfhat*lambda 0]))) | (badg & (f0-f) < 0) ;
      growSignal = ~badg & ( (lambda > 0)  &  (f0-f > -(1-THETA)*dfhat*lambda) );
      if  shrinkSignal  &   ( (lambda>lambdaPeak) | (lambda<0) )
         if (lambda>0) & ((~shrink) | (lambda/factor <= lambdaPeak))
            shrink=1;
            factor=factor^.6;
            while lambda/factor <= lambdaPeak
               factor=factor^.6;
            end
            %if (abs(lambda)*(factor-1)*dxnorm < MINDX) | (abs(lambda)*(factor-1) < MINLAMB)
            if abs(factor-1)<MINDFAC
               if abs(lambda)<4
                  retcode=2;
               else
                  retcode=7;
               end
               done=1;
            end
         end
         if (lambda<lambdaMax) & (lambda>lambdaPeak)
            lambdaMax=lambda;
         end
         lambda=lambda/factor;
         if abs(lambda) < MINLAMB
            if (lambda > 0) & (f0 <= fhat)
               % try going against gradient, which may be inaccurate
               lambda = -lambda*factor^6;
            else
               if lambda < 0
                  retcode = 6;
               else
                  retcode = 3;
               end
               done = 1;
            end
         end
      elseif  (growSignal & lambda>0) |  (shrinkSignal & ((lambda <= lambdaPeak) & (lambda>0)))
         if shrink
            shrink=0;
            factor = factor^.6;
            %if ( abs(lambda)*(factor-1)*dxnorm< MINDX ) | ( abs(lambda)*(factor-1)< MINLAMB)
            if abs(factor-1)<MINDFAC
               if abs(lambda)<4
                  retcode=4;
               else
                  retcode=7;
               end
               done=1;
            end
         end
         if ( f<fPeak ) & (lambda>0)
            fPeak=f;
            lambdaPeak=lambda;
            if lambdaMax<=lambdaPeak
               lambdaMax=lambdaPeak*factor*factor;
            end
         end
         lambda=lambda*factor;
         if abs(lambda) > 1e20;
            retcode = 5;
            done =1;
         end
      else
         done=1;
         if factor < 1.2
            retcode=7;
         else
            retcode=0;
         end
      end
   end
end


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 parent = connectedComponents(A)
% label connected componentes in an graph (adjacency matrix)

%elimination tree
parent = etree(A);

isleaf = parent ~= 0;
parent(~isleaf) = 1:nnz(~isleaf);
i = find(isleaf);
for i = i(end:-1:1)
  parent(i) = parent(parent(i));
end


function vars(vh)

num_masses = vh.userdata.num_masses;

lbx = horzcat(-4*ones(1,num_masses), -inf*ones(1,num_masses)).';
ubx = horzcat(4*ones(1,num_masses), inf*ones(1,num_masses)).';

vh.addState('x', [2*num_masses 1], 'lb', lbx, 'ub', ubx);
vh.addControl('u', [num_masses-1 1], 'lb', -0.5, 'ub', 0.5);


function mse = EvalMSE(NN,tNN,sqi,HRVparams,windows_all)

% mse = EvalMSE(NN,tNN,sqi,HRVparams,windows_all)
%
%   OVERVIEW:   This function returns MultiScale Entropy calculated on
%               input NN intervals for each window.
%
%   INPUT:      MANDATORY:
%               NN             : a single row of NN (normal normal) interval
%                                data in seconds
%               tNN            : the time indices of the NN interval data
%                                (seconds)
%               sqi            : (Optional )Signal Quality Index; Requires
%                                a matrix with at least two columns. Column
%                                1 should be timestamps of each sqi measure,
%                                and Column 2 should be SQI on a scale from 0 to 1.
%               HRVparams      : struct of settings for hrv_toolbox analysis
%               windows_all    : vector containing the starting time of each
%                                windows (in seconds)
%
%   OUTPUT:
%               mse            : vector of [max_tau, 1] doubles for each
%                                window
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   ORIGINAL SOURCE AND AUTHORS:
%       Written by Giulia Da Poian
%	COPYRIGHT (C) 2016
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information
%

% Verify input arguments
if nargin < 4
    error('Wrong number of input parameters');
end
if nargin < 5 || isempty(windows_all)
    windows_all = 0;
end
if isempty(sqi)
     sqi(:,1) = tNN;
     sqi(:,2) = ones(length(tNN),1);
end
% Set Defaults

if isempty(HRVparams.MSE.windowlength)
    windowlength = length(NN);
else
    windowlength = HRVparams.MSE.windowlength*3600;
end

threshold1 = HRVparams.sqi.LowQualityThreshold;
threshold2 = HRVparams.RejectionThreshold;

m = HRVparams.MSE.patternLength;
r = HRVparams.MSE.RadiusOfSimilarity;
maxTau = HRVparams.MSE.maxCoarseGrainings;

cg_moment = HRVparams.MSE.moment;
cg_method = HRVparams.MSE.method;
constant_r = HRVparams.MSE.constant_r;

% Preallocate arrays (all NaN) before entering the loop
mse = nan(maxTau,length(windows_all));

%Analyze by Window

% Loop through each window of RR data
for i_win = 1:length(windows_all)
    % Check window for sufficient data
    if ~isnan(windows_all(i_win))
        % Isolate data in this window
        idx_NN_in_win = find(tNN >= windows_all(i_win) & tNN < windows_all(i_win) + windowlength);
        idx_sqi_win = find(sqi(:,1) >= windows_all(i_win) & sqi(:,1) < windows_all(i_win) + windowlength);

        sqi_win = sqi(idx_sqi_win,:);
        nn_win = NN(idx_NN_in_win);

        % Analysis of SQI for the window
        lowqual_idx = find(sqi_win(:,2) < threshold1);

        % If enough data has an adequate SQI, perform the calculations
        if numel(lowqual_idx)/length(sqi_win(:,2)) < threshold2
            mse(:,i_win) = ComputeMultiscaleEntropy(nn_win, m, r, maxTau, [], cg_moment,cg_method,constant_r);
        end

    end % end check for sufficient data

end % end of loop through window

end % end of function


function f_x = ParFor5(in1)
%PARFOR5
%    F_X = PARFOR5(IN1)

%    This function was generated by the Symbolic Math Toolbox version 8.2.
%    20-Sep-2019 09:35:35

u = in1(:,1);
ux = in1(:,4);
uxx = in1(:,5);
uxxx = in1(:,6);
f_x = ((u.*7.604652919865344e15+ux.*1.439296854415953e19+uxx.*4.056295991410688e16-uxxx.*3.942428646215516e17-u.*ux.*1.138844935991067e19-u.*uxxx.*1.804892689519346e18+2.199453572808704e15).*-1.0)./(u.*4.656689920376832e15-1.151078656075366e17);


function [aps, recall, precision] = calcAP(track_confs, tp_cell, fp_cell, num_vids, num_track_per_class, num_track_thr, defaultTrackThr)

fprintf('computing AP\n');
recall = cell(1,num_track_thr);
precision = cell(1,num_track_thr);
aps = cell(1,num_track_thr);
confs = [track_confs{:}];
[~, ind] = sort(confs,'descend');
for o = 1:num_track_thr
    tp_all = [];
    fp_all = [];
    for v = 1:num_vids
        tp_all = [tp_all(:); tp_cell{v}{o}'];
        fp_all = [fp_all(:); fp_cell{v}{o}'];
    end

    tp_all = tp_all(ind)';
    fp_all = fp_all(ind)';

    % compute precision/recall
    tp = cumsum(tp_all);
    fp = cumsum(fp_all);
    recall{o} = (tp/num_track_per_class)';
    precision{o} = (tp./(fp+tp))';
    aps{o} = VOCap(recall{o},precision{o})*100;
end

fprintf('-------------\n');
ap = aps{1};
for t = 2:length(aps)
    ap = ap + aps{t};
end
ap = ap ./ length(aps);
fprintf('Mean AP:\t\t %0.2f%%\n',mean(ap));
fprintf(' = = = = = = = = \n');
for t = 1:length(aps)
    ap = aps{t};
    fprintf('Mean AP@%0.2f:\t %0.2f%%\n',defaultTrackThr(t),mean(ap));
end
fprintf(' = = = = = = = = \n');


%%% Nowcasting %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This script produces a nowcast of US real GDP growth for 2016:Q4
% using the estimated parameters from a dynamic factor model.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Clear workspace and set paths.
close all; clear; clc;
addpath('functions');

%% User inputs.
series = 'GDPC1' ; % Nowcasting real GDP (GDPC1) <fred.stlouisfed.org/series/GDPC1>
period = '2016q4'; % Forecasting target quarter

%% Load model specification and first vintage of data.
% Load model specification structure `Spec`
Spec = load_spec('Spec_US_example.xls');

%% Load DFM estimation results structure `Res`.
Res = load('ResDFM'); % example_DFM.m used the first vintage of data for estimation

%% Update nowcast and decompose nowcast changes into news.

%%% Nowcast update from week of December 7 to week of December 16, 2016 %%%
vintage_old = '2016-12-16'; datafile_old = fullfile('data','US',[vintage_old '.xls']);
vintage_new = '2016-12-23'; datafile_new = fullfile('data','US',[vintage_new '.xls']);
% Load datasets for each vintage
[X_old,~   ] = load_data(datafile_old,Spec);
[X_new,Time] = load_data(datafile_new,Spec);

% check if spec used in estimation is consistent with the current spec
if isequal(Res.Spec,Spec)
    Res = Res.Res;
else
    threshold = 1e-4; % Set to 1e-5 for robust estimates
    Res = dfm(X_new,Spec,threshold);
    save('ResDFM','Res','Spec');
end

update_nowcast(X_old,X_new,Time,Spec,Res,series,period,vintage_old,vintage_new);

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


function [R,C] = syncRot(T)

[~,L,Q] = proj_deformable_approx(T');
s = sign(L(find(abs(L)==max(abs(L)),1)));
C = s*L';
R = s*Q';
R(3,:) = cross(R(1,:),R(2,:));

% [R,C] = projectNonrigid(T);
% R(3,:) = cross(R(1,:),R(2,:));


function neg_tmpl =  sample_neg(frame, p, out_size, margin)
    %% Sample 16 negative image samples arround the estimated bounding box with 2 different scales each
    % the parameter margin specifices the distance scale between the original bounding box and the sampled negative boxes
    count = 1;
    p_ori = p;
    len = (p(3) + p(4)) / 2;
    for context_scale = [0.5, 1]
        for i = -1 : 1
            for j = -1 : 1
                if i == 0 && j == 0
                    continue;
                end
                tx = i * (p_ori(3) / 2 + len * (context_scale + min(margin / len, 0.1)));
                ty = j * (p_ori(4) / 2 + len * (context_scale + min(margin / len, 0.1)));
                p = p_ori;
                p(1) = p(1) + tx;
                p(2) = p(2) + ty;
                p(3) = 2 * len * context_scale;
                p(4) = 2 * len * context_scale;
                crop_img = im_crop(frame, round(p), out_size);
                neg_tmpl(:, count) = crop_img(:);
                count = count + 1;

                if i == 0
                    p(1) = p(1) - len * context_scale;
                    crop_img = im_crop(frame, round(p), out_size);
                    neg_tmpl(:, count) = crop_img(:);
                    count = count + 1;

                    p(1) = p(1) + 2 * len * context_scale;
                    crop_img = im_crop(frame, round(p), out_size);
                    neg_tmpl(:, count) = crop_img(:);
                    count = count + 1;
                end

                if j == 0
                    p(2) = p(2) - len * context_scale;
                    crop_img = im_crop(frame, round(p), out_size);
                    neg_tmpl(:, count) = crop_img(:);
                    count = count + 1;

                    p(2) = p(2) + 2 * len * context_scale;
                    crop_img = im_crop(frame, round(p), out_size);
                    neg_tmpl(:, count) = crop_img(:);
                    count = count + 1;
                end
            end
        end
    end

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');


function [cq mcq Xc]=getmeanquality(X,c,linkagetype)

    %point estimate cluster solution
    Y = pdist1(X);

    if exist('linkage.m', 'file') && exist('cluster.m', 'file')
        Z = linkage(Y,linkagetype);
        Xc = cluster(Z,c);
    else
        Z = linkage_t(Y,linkagetype);
        Xc = cluster_t(Z,c);
    end

    %convert X to X, binary input to doquality
    Xcx=makebinary(Xc)';

    %point estimate quality of each element;
    [cq center]=clustquality(Xcx,X);      %cq is cluster quality of each element

    %mean cq
    if all(cq == 1)
        mcq = 0;
    else
        mcq = mean( cq(cq~=1) );    %don't include single element clusters in your quality estimate
    end

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 [ hitsclean, polyidsclean, rayidsclean ] = invalidateintersections( pos_IMUinG, hits, polyids, rayids )
%INVALIDATEINTERSECTIONS Will handle that a single ray hits many planes
%   We will go through each rayid and from there we will check to see which
%   of the given intersections for that ray are closest to the given
%   position of the LIDAR. We know that the closest ray would intersect
%   first so that is the only valid ray intersection.

% Sort the arrays based on the rayid
[rayids, indices] = sort(rayids);
hits = hits(indices,:);
polyids = polyids(indices,:);

% Array to keep track of the max distance from our pos
maxdist = inf(1,max(rayids));
maxids = zeros(1,max(rayids));

% Now we know we are sorted, so lets loop through each one and find the
% shortest distance for the given ray ids
for ii=1:size(rayids,1)
    % compute the square difference between this point and the LIDAR pose
    diff = sqrt(sum((pos_IMUinG-hits(ii,1:3)).^2));
    % next lets see if it is the new min
    if diff < maxdist(rayids(ii,1))
        maxdist(rayids(ii,1)) = diff;
        maxids(rayids(ii,1)) = ii;
    end
end

% our output args
hitsclean = [];
polyidsclean = [];
rayidsclean = [];

% finally lets create the return values
cleanct = 0;
for ii=1:size(maxdist,2)
    % if it is not inf then we have the min
    if maxdist(1,ii) ~= Inf
        hitsclean = [hitsclean; hits(maxids(1,ii),:)];
        polyidsclean = [polyidsclean; polyids(maxids(1,ii),:)];
        rayidsclean = [rayidsclean; rayids(maxids(1,ii),:)];
        cleanct = cleanct + 1;
    end
end

% Debug info
fprintf('RAY: %d left after invalidation.\n',cleanct)

end


function Offspring = Operator(Problem,Particle,Pbest,Gbest)
% <operator> <real>
% Particle swarm optimization in NMPSO

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

    %% Parameter setting
    ParticleDec = Particle.decs;
    PbestDec    = Pbest.decs;
    GbestDec    = Gbest.decs;
    [N,D]       = size(ParticleDec);
    ParticleVel = Particle.adds(zeros(N,D));

    %% Particle swarm optimization
    W  = repmat(unifrnd(0.1,0.5,N,1),1,D);
    r1 = repmat(rand(N,1),1,D);
    r2 = repmat(rand(N,1),1,D);
    r3 = repmat(rand(N,1),1,D);
    C1 = repmat(unifrnd(1.5,2.5,N,1),1,D);
    C2 = repmat(unifrnd(1.5,2.5,N,1),1,D);
    C3 = repmat(unifrnd(1.5,2.5,N,1),1,D);
    OffVel    = W.*ParticleVel + C1.*r1.*(PbestDec-ParticleDec) + C2.*r2.*(GbestDec-ParticleDec) + C3.*r3.*(GbestDec-PbestDec);
    OffDec    = ParticleDec + OffVel;
    Offspring = Problem.Evaluation(OffDec,OffVel);
end


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.)


function target = myConvolve(kConv, data, kernel, stride, task)
% Matlab interfaces of alex fast convolution kernels.
% There are are total 3 kinds of operations : given the two of (images,
% filters, hidacts) to compute the remaining one. And for each operation,
% there are two kernels : one for the second layer which is optimized
% for one channel input and one for the other layers.

% forward:  (data,kernel) == (images, filters)
% weight:   (data,kernel) == (images, hidacts)
% backward: (data,kernel) == (hidacts, filters)

% interfaces for cuda code kFunctions.cu.

if strcmp(task,'forward')
    numColors = size(data,5);
    if numColors == 1
        numFilters = size(kernel,1); filter_batch_size = 16;
        filter_batch_num = numFilters / filter_batch_size;
        assert(filter_batch_num == floor(filter_batch_num));

        numImages = size(data,1); imgSizeX = size(data,2); imgSizeY = size(data,3); imgSizeZ = size(data,4);
        filterSize =  size(kernel,2);

        paddingStart = 0; moduleStride = stride; imgStride = numImages;
        numModulesX = (imgSizeX - filterSize) / stride + 1; numModulesY = (imgSizeY - filterSize) / stride + 1; numModulesZ = (imgSizeZ - filterSize) / stride + 1;
        target = zeros(numImages, numModulesX, numModulesY, numModulesZ, numFilters, 'single');
        for b = 1 : filter_batch_num
            filterPerThread = 4; imagePerThread = 1;
            kConv.ThreadBlockSize = [32, 4];
            kConv.GridSize = [ceil(numImages/(32 * imagePerThread)), numModulesX * numModulesY * numModulesZ * filter_batch_size / (filterPerThread * 4) ]; % filterPerThread, BlockSize.Y
            target_gpu = feval(kConv,...
               target(:,:,:,:,(b-1)*filter_batch_size+1 : b*filter_batch_size), data, kernel((b-1)*filter_batch_size+1 : b*filter_batch_size,:,:,:,:),...
               numImages, filter_batch_size, imgSizeZ, imgSizeY, imgSizeX, filterSize, ...
               paddingStart, moduleStride, numModulesZ, numModulesY, numModulesX, imgStride);
           target(:,:,:,:,(b-1)*filter_batch_size+1 : b*filter_batch_size) = gather(target_gpu);
        end
    else
        numImages = size(data,1); imgSizeX = size(data,2); imgSizeY = size(data,3); imgSizeZ = size(data,4);
        numFilters = size(kernel,1); filterSize =  size(kernel,2);

        paddingStart = 0; moduleStride = stride; imgStride = numImages; numGroups = 1;
        numModulesX = (imgSizeX - filterSize) / stride + 1; numModulesY = (imgSizeY - filterSize) / stride + 1; numModulesZ = (imgSizeZ - filterSize) / stride + 1;

        filterPerThread = 8; imagePerThread = 1;

        kConv.ThreadBlockSize = [32, 4];
        kConv.GridSize = [ceil(numImages/(32 * imagePerThread)), numModulesX * numModulesY * numModulesZ * numFilters / (filterPerThread * 4) ]; % filterPerThread, BlockSize.Y

        target = zeros(numImages, numModulesX, numModulesY, numModulesZ, numFilters, 'single');

        target_gpu = feval(kConv,...
           target, data, kernel,...
           numImages, numFilters, imgSizeZ, imgSizeY, imgSizeX, filterSize, ...
           paddingStart, moduleStride, numModulesZ, numModulesY, numModulesX, imgStride, numColors, numGroups);

        target = gather(target_gpu);
    end

elseif strcmp(task,'weight')
    numColors = size(data,5);
    if numColors == 1
        numImages = size(data,1); imgSizeX = size(data,2); imgSizeY = size(data,3); imgSizeZ = size(data,4);
        numModulesX = size(kernel,2); numModulesY = size(kernel,3); numModulesZ = size(kernel,4); numFilters = size(kernel,5);
        paddingStart = 0; moduleStride = stride; imgStride = numImages; partialSum = numModulesX * numModulesY * numModulesZ;
        filterSize = imgSizeX - stride * (numModulesX - 1);

        pixelsPerThread = 5; preLoadCases = 32; scaleOutput = 1 ./ (numImages * partialSum);

        kConv.ThreadBlockSize = [16, 8];
        kConv.GridSize = [numFilters*numModulesX*numModulesY*numModulesZ/partialSum/16, ceil(filterSize^3 /(8*pixelsPerThread))];

        target = zeros(numFilters, filterSize, filterSize, filterSize, numColors, 'single');
        target_gpu = feval(kConv, ....
           target, data, kernel,...
           numImages, numFilters, numModulesZ, numModulesY, numModulesX, imgSizeZ, imgSizeY, imgSizeX, ...
           filterSize, paddingStart, moduleStride, imgStride, partialSum, scaleOutput);

        target = gather(target_gpu);
    else
        numImages = size(data,1); imgSizeX = size(data,2); imgSizeY = size(data,3); imgSizeZ = size(data,4);
        numFilters = size(kernel,5); numModulesX = size(kernel,2); numModulesY = size(kernel,3); numModulesZ = size(kernel,4);
        paddingStart = 0; moduleStride = stride; imgStride = numImages; partialSum = numModulesX * numModulesY * numModulesZ;
        filterSize = imgSizeX - stride * (numModulesX - 1);

        preLoadCases = 32; filtersPerThread = 2; colorsPerThread = 8;
        scaleOutput = 1 ./ (numImages * partialSum); numGroups = 1;

        kConv.ThreadBlockSize = [16, 8];
        kConv.GridSize = [numFilters*numModulesX*numModulesY*numModulesZ/partialSum/16/filtersPerThread, ceil(filterSize^3 / 8) * (numColors / colorsPerThread)];

        target = zeros(numFilters, filterSize, filterSize, filterSize, numColors, 'single');
        target_gpu = feval(kConv, ....
           target, data, kernel,...
           numImages, numFilters, numModulesZ, numModulesY, numModulesX, imgSizeZ, imgSizeY, imgSizeX, ...
           filterSize, paddingStart, moduleStride, imgStride, numColors, numGroups, partialSum, scaleOutput);

        target = gather(target_gpu);
    end
elseif strcmp(task,'backward')
    numColors = size(kernel,5);
    if numColors == 1
        numImages = size(data,1); numModulesX = size(data,2); numModulesY = size(data,3); numModulesZ = size(data,4); moduleStride = stride;
        numFilters = size(kernel,1); filterSize = size(kernel,2);

        imgSizeX = stride * (numModulesX - 1) + filterSize; imgSizeY = stride * (numModulesY - 1) + filterSize; imgSizeZ = stride * (numModulesZ - 1) + filterSize;
        paddingStart = 0; imgsPerThread = 2;

        kConv.ThreadBlockSize = [16, 16];
        kConv.GridSize = [ceil(numImages/(imgsPerThread *16)), imgSizeZ * ceil(imgSizeY/4) * ceil(imgSizeX/4)];

        target = zeros(numImages, imgSizeX, imgSizeY, imgSizeZ, numColors, 'single');
        target_gpu = feval(kConv,....
            target, data, kernel,...
            numModulesZ, numModulesY, numModulesX, numImages, numFilters, filterSize, ...
            imgSizeZ, imgSizeY, imgSizeX, paddingStart, moduleStride);

        target = gather(target_gpu);
    else
        numImages = size(data,1); numModulesX = size(data,2); numModulesY = size(data,3); numModulesZ = size(data,4); moduleStride = stride;
        numFilters = size(kernel,1); filterSize = size(kernel,2);

        imgSizeX = stride * (numModulesX - 1) + filterSize; imgSizeY = stride * (numModulesY - 1) + filterSize; imgSizeZ = stride * (numModulesZ - 1) + filterSize;
        paddingStart = 0; numGroups = 1;

        colorsPerThread = 4; imgsPerThread = 1;

        kConv.ThreadBlockSize = [32, 4];
        kConv.GridSize = [ceil(numImages/(imgsPerThread * 32)) * (numColors / (4 * colorsPerThread)), imgSizeZ * imgSizeY * imgSizeX];

        target = zeros(numImages, imgSizeX, imgSizeY, imgSizeZ, numColors, 'single');
        target_gpu = feval(kConv,....
            target, data, kernel,...
            numModulesZ, numModulesY, numModulesX, numImages, numFilters, filterSize, ...
            imgSizeZ, imgSizeY, imgSizeX, paddingStart, moduleStride, numColors, numGroups);

        target = gather(target_gpu);
    end
end


%%*******************************************************************
%% 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;
%%*******************************************************************


% This m-file takes care of synthetic division.
% By giving one polynomial and one root this function returns
% the polynomial formed with the other roots of the given polynomial excluding the given root.
% Keerthi Venkateswara Rao
function coeff_second=syn_division(coeff_function,fun_root_new)
order_fun=size((coeff_function),2);
coeff_second=0;
for index=1:size((coeff_function),2)-1
    if index==1
        coeff_second(index)=coeff_function(index);
    else
        coeff_second(index)=coeff_function(index)+fun_root_new*coeff_second(index-1);
    end
end


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 [p] = config_hog3x3(c)
%
% Copyright Aditya Khosla http://mit.edu/khosla
%
% Please cite this paper if you use this code in your publication:
%   A. Khosla, J. Xiao, A. Torralba, A. Oliva
%   Memorability of Image Regions
%   Advances in Neural Information Processing Systems (NIPS) 2012
%

p.grid_spacing = 6; % distance between grid centers
p.patch_size = 24;
p.Mw = 2; % number of spatial scales for spatial pyramid histogram
p.descriptor = 'hog';
p.w = p.patch_size; % boundary for HOG

p.pyramid_levels = 2;
p.llcknn = 3;
p.maxsize = 400;

% dictionary parameters
p.dictionary_size = 256;
p.num_images = 500;
p.descPerImage = 2000;
p.num_desc = 500000;

p.train_file = '%s/train_hog3x3_%d.mat';
p.test_file = '%s/test_hog3x3_%d.mat';
p.dictionary_file = '%s/dictionary_hog3x3_%d.mat';


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


Wd = function (matvec)
%take a 12 param matvec, typically WARPDRIVE_MATVEC_INV_000000 and
%change it to a WARP_DATA
%Used to test transformations needed to make AFNI use 3dWarpDrive's output
%as a TLRC transform

if (nargin == 0) matvec = [];

if (isempty(matvec)),
   WARNING: Using preset matvecs!\n\n');
   %test case, matvecs from DemoSubj_SurfVol_Alnd_Exp_at2+tlrc
   if (0),
      matvec = [  0.9709206    -0.04199445     0.01165925     -0.7014965
               0.07297896    0.9860933    -0.08915191     -0.2648731
               0.005342953    -0.02306779  0.9538299      -6.124281
               ];
   else
      %that would be WARPDRIVE_MATVEC_INV_000000
      %the proper one to use for WARP_DATA, the one that takes you
      %from +orig to +tlrc
      matvec = [  1.026725     0.04352641   -0.008482005      0.6798272
                  -0.07667368  1.013075     0.09562658      0.8001939
                  -0.007605589   0.02425676 1.050765       6.436271
               ];
   end
else
   if (length(matvec) ~= 12),
      fprintf(2,'Error: matvec must be 12 elements long\n');
   end
end

Mfor = [matvec; 0 0 0 1]; %the forward transform
Mbac = inv(Mfor); %the inverse transform

%begin transformation
mfor = Mfor(1:3, 1:3);
mbac = Mbac(1:3, 1:3);
bvec = -Mfor(1:3,4); %Don't ask about the minus sign, see README.attributes:
                     %Under WARP_DATA, The forward transformation is  [x_map] = [mfor] [x_in]  - [bvec];
                     %Under Registration Attributes: [xyz_out] = [mat] ([xyz_in]-[xyz_cen]) + [vec] + [xyz_cen]
svec = -Mbac(1:3,4);
bot = [-80 ; -80 ; -65]; %good enough for TT box
top = [80 ; 110 ; 85];

Wd = [reshape(mfor',1,9) reshape(mbac',1,9) reshape(bvec,1,3) reshape(svec,1,3) [bot'] [top']];

fprintf(1,'Wd = \n\n');
fprintf(1,'       %g   %g   %g   %g   %g   %g\n',Wd)
fprintf(1,'\n\n');


function cost = costSymbolSeparation(par, sampleRate, iq)

symbols = computeSymbolsFromParams(par./[1 1 100 100], sampleRate, iq);

%%
aa = mod(angle(symbols),(pi/4));
bb = mod(abs(symbols),.25);

%%
aa = min([aa; pi/4-aa]);
bb = min([bb; .25-bb]);

%%
cost = sum([aa.^2, bb.^2]);


function problem = prepare_speed_problem(speedpath,speedimgpath,imgidx,nrOutliers)
load([speedpath,'keypoints-2.mat']);
imgs         = dir([speedimgpath,'*.jpg']);
nrImgs       = length(imgs);
model_3d     = double( model_3d' * 0.00000586 );
nrLandmarks  = 11;
camera_K     = [3003.41, 0, 960;
                0, 3003.41, 600;
                0, 0, 1];
camera_Kinv  = inv(camera_K);
img          = imread([speedimgpath,imgs(imgidx).name]);
keypoints    = double( squeeze(prediction_2d(imgidx,:,:))' );
keypoints(:,1:nrOutliers) = [1920 * rand(1,nrOutliers); 1200 * rand(1,nrOutliers)];
keypoints_2d = camera_Kinv * [keypoints;ones(1,nrLandmarks)];
keypoints_2d = keypoints_2d(1:2,:);
R_gt         = quat2rotm( gt_pose(imgidx,1:4) );
t_gt         = gt_pose(imgidx,5:end)';

problem.N    = nrLandmarks;
problem.X    = model_3d;
problem.x    = keypoints_2d;
problem.FOV  = 90;
problem.translationBound = 25.0;
problem.noiseBoundSq     = (0.2)^2;
problem.noiseBound       = sqrt(problem.noiseBoundSq);
problem.depthBound       = 0.0;
problem.pixNoiseBound    = 1e-2;
problem.R_gt             = R_gt;
problem.t_gt             = t_gt;
problem.img              = img;

problem.keypoints_pix    = keypoints;

problem.camera_K         = camera_K;

fprintf('SPEED: N: %d, num outliers: %d, noiseBound: %g.\n',...
    problem.N,nrOutliers,problem.noiseBound);

if ~check_translation(problem.t_gt,problem.translationBound,problem.FOV)
    error('Problem assumption wrong.')
end

end


function [results] = trackerMain(p, im, bg_area, fg_area, area_resize_factor)
%TRACKERMAIN contains the main loop of the tracker, P contains all the parameters set in runTracker
    %% INITIALIZATION
    num_frames = numel(p.img_files);
    % used for OTB-13 benchmark
    OTB_rect_positions = zeros(num_frames, 4);
	pos = p.init_pos;
    target_sz = p.target_sz;
	num_frames = numel(p.img_files);
    % patch of the target + padding
    patch_padded = getSubwindow(im, pos, p.norm_bg_area, bg_area);
    % initialize hist model
    new_pwp_model = true;
    [bg_hist, fg_hist] = updateHistModel(new_pwp_model, patch_padded, bg_area, fg_area, target_sz, p.norm_bg_area, p.n_bins, p.grayscale_sequence);
    new_pwp_model = false;
    % Hann (cosine) window
    if isToolboxAvailable('Signal Processing Toolbox')
        hann_window = single(hann(p.cf_response_size(1)) * hann(p.cf_response_size(2))');
    else
        hann_window = single(myHann(p.cf_response_size(1)) * myHann(p.cf_response_size(2))');
    end
    % gaussian-shaped desired response, centred in (1,1)
    % bandwidth proportional to target size
    output_sigma = sqrt(prod(p.norm_target_sz)) * p.output_sigma_factor / p.hog_cell_size;
    y = gaussianResponse(p.cf_response_size, output_sigma);
    yf = fft2(y);

    %Context patches
    offset = [-target_sz(1) 0; 0 -target_sz(2); target_sz(1) 0; 0 target_sz(2)];
    elapsed_time =0;

    %% SCALE ADAPTATION INITIALIZATION
    if p.scale_adaptation
        % Code from DSST
        scale_factor = 1;
        base_target_sz = target_sz;
        scale_sigma = sqrt(p.num_scales) * p.scale_sigma_factor;
        ss = (1:p.num_scales) - ceil(p.num_scales/2);
        ys = exp(-0.5 * (ss.^2) / scale_sigma^2);
        ysf = single(fft(ys));
        if mod(p.num_scales,2) == 0
            scale_window = single(hann(p.num_scales+1));
            scale_window = scale_window(2:end);
        else
            scale_window = single(hann(p.num_scales));
        end;

        ss = 1:p.num_scales;
        scale_factors = p.scale_step.^(ceil(p.num_scales/2) - ss);

        if p.scale_model_factor^2 * prod(p.norm_target_sz) > p.scale_model_max_area
            p.scale_model_factor = sqrt(p.scale_model_max_area/prod(p.norm_target_sz));
        end

        scale_model_sz = floor(p.norm_target_sz * p.scale_model_factor);
        % find maximum and minimum scales
        min_scale_factor = p.scale_step ^ ceil(log(max(5 ./ bg_area)) / log(p.scale_step));
        max_scale_factor = p.scale_step ^ floor(log(min([size(im,1) size(im,2)] ./ target_sz)) / log(p.scale_step));
    end

    %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%
    %% MAIN LOOP
    for frame = 1:num_frames

        im = imread(fullfile(p.img_path, p.img_files{frame}));
        tic()

        if frame>1

	    %% TESTING step
            % extract patch of size bg_area and resize to norm_bg_area
            im_patch_cf = getSubwindow(im, pos, p.norm_bg_area, bg_area);
            pwp_search_area = round(p.norm_pwp_search_area / area_resize_factor);
            % extract patch of size pwp_search_area and resize to norm_pwp_search_area
            im_patch_pwp = getSubwindow(im, pos, p.norm_pwp_search_area, pwp_search_area);
            % compute feature map
            xt = getFeatureMap(im_patch_cf, p.feature_type, p.cf_response_size, p.hog_cell_size);
            % apply Hann window
            xt_windowed = bsxfun(@times, hann_window, xt);
            % compute FFT
            xtf = fft2(xt_windowed);
            % Correlation between filter and test patch gives the response
            % Solve diagonal system per pixel.
            if p.den_per_channel
% 				hf = hf_num ./ (hf_den + p.lambda);
                hf = hf_num ./ hf_den;
            else
				%hf = bsxfun(@rdivide, hf_num, sum(hf_den, 3)+p.lambda);
                hf = bsxfun(@rdivide, hf_num, sum(hf_den, 3));
            end
            %response_cf = ensure_real(ifft2(sum(conj(hf) .* xtf, 3)));
            response_cf = ensure_real(ifft2(sum(hf .* xtf, 3)));

            % Crop square search region (in feature pixels).
            response_cf = cropFilterResponse(response_cf, ...
                floor_odd(p.norm_delta_area / p.hog_cell_size));
            if p.hog_cell_size > 1
                % Scale up to match center likelihood resolution.
                response_cf = mexResize(response_cf, p.norm_delta_area,'auto');
                %response_cf = imresize(response_cf, p.norm_delta_area,'nearest');
            end

            [likelihood_map] = getColourMap(im_patch_pwp, bg_hist, fg_hist, p.n_bins, p.grayscale_sequence);
            % (TODO) in theory it should be at 0.5 (unseen colors shoud have max entropy)
            likelihood_map(isnan(likelihood_map)) = 0;

            % each pixel of response_pwp loosely represents the likelihood that
            % the target (of size norm_target_sz) is centred on it
            response_pwp = getCenterLikelihood(likelihood_map, p.norm_target_sz);

            %% ESTIMATION
            response = mergeResponses(response_cf, response_pwp, p.merge_factor, p.merge_method);
            [row, col] = find(response == max(response(:)), 1);
            center = (1+p.norm_delta_area) / 2;
            pos = pos + ([row, col] - center) / area_resize_factor;
            rect_position = [pos([2,1]) - target_sz([2,1])/2, target_sz([2,1])];

            %% SCALE SPACE SEARCH
            if p.scale_adaptation
                im_patch_scale = getScaleSubwindow(im, pos, base_target_sz, scale_factor * scale_factors, scale_window, scale_model_sz, p.hog_scale_cell_size);
                xsf = fft(im_patch_scale,[],2);
                scale_response = real(ifft(sum(sf_num .* xsf, 1) ./ (sf_den + p.lambda) ));
                recovered_scale = ind2sub(size(scale_response),find(scale_response == max(scale_response(:)), 1));
                %set the scale
                scale_factor = scale_factor * scale_factors(recovered_scale);

                if scale_factor < min_scale_factor
                    scale_factor = min_scale_factor;
                elseif scale_factor > max_scale_factor
                    scale_factor = max_scale_factor;
                end
                % use new scale to update bboxes for target, filter, bg and fg models
                target_sz = round(base_target_sz * scale_factor);
                avg_dim = sum(target_sz)/2;
                bg_area = round(target_sz + avg_dim);
                if(bg_area(2)>size(im,2)),  bg_area(2)=size(im,2)-1;    end
                if(bg_area(1)>size(im,1)),  bg_area(1)=size(im,1)-1;    end

                bg_area = bg_area - mod(bg_area - target_sz, 2);
                fg_area = round(target_sz - avg_dim * p.inner_padding);
                fg_area = fg_area + mod(bg_area - fg_area, 2);
                % Compute the rectangle with (or close to) params.fixed_area and
                % same aspect ratio as the target bboxgetScaleSubwindow
                area_resize_factor = sqrt(p.fixed_area/prod(bg_area));
            end

            if p.visualization_dbg==1
                mySubplot(2,1,5,1,im_patch_cf,'FG+BG','gray');
                mySubplot(2,1,5,2,likelihood_map,'obj.likelihood','parula');
                mySubplot(2,1,5,3,response_cf,'CF response','parula');
                mySubplot(2,1,5,4,response_pwp,'center likelihood','parula');
                mySubplot(2,1,5,5,response,'merged response','parula');
                drawnow
            end
        end

        %% TRAINING
        % extract patch of size bg_area and resize to norm_bg_area
        im_patch_bg = getSubwindow(im, pos, p.norm_bg_area, bg_area);
        % compute feature map, of cf_response_size
        xt = getFeatureMap(im_patch_bg, p.feature_type, p.cf_response_size, p.hog_cell_size);
        % apply Hann window
        xt = bsxfun(@times, hann_window, xt);
        % compute FFT
        xtf = fft2(xt);
        kf = (conj(xtf) .* xtf); %/ prod(p.cf_response_size);

        kfn = zeros([size(xtf) length(offset)]);
        for j=1:length(offset)
        %obtain a subwindow close to target for regression to 0
		im_patch_bgn = getSubwindow(im, pos+offset(j,:), p.norm_bg_area, bg_area);
        % compute feature map, of cf_response_size
        xtn = getFeatureMap(im_patch_bgn, p.feature_type, p.cf_response_size, p.hog_cell_size);
        % apply Hann window
        xtn = bsxfun(@times, hann_window, xtn);
        % compute FFT
        xtfn = fft2(xtn);
        kfn(:,:,:,j) = conj(xtfn) .*xtfn; %/ prod(p.cf_response_size);
        end

        %% FILTER UPDATE
        % Compute expectations over circular shifts,
        % therefore divide by number of pixels.
		new_hf_num = bsxfun(@times, conj(xtf),yf); %/ prod(p.cf_response_size);
		new_hf_den = kf + p.lambda + p.lambda2.*sum(kfn,4);

        if frame == 1
            % first frame, train with a single image
		    hf_den = new_hf_den;
		    hf_num = new_hf_num;
		else
		    % subsequent frames, update the model by linear interpolation
        	hf_den = (1 - p.learning_rate_cf) * hf_den + p.learning_rate_cf * new_hf_den;
	   	 	hf_num = (1 - p.learning_rate_cf) * hf_num + p.learning_rate_cf * new_hf_num;

            %% BG/FG MODEL UPDATE
            % patch of the target + padding
            [bg_hist, fg_hist] = updateHistModel(new_pwp_model, im_patch_bg, bg_area, fg_area, target_sz, p.norm_bg_area, p.n_bins, p.grayscale_sequence, bg_hist, fg_hist, p.learning_rate_pwp);
        end

        %% SCALE UPDATE
        if p.scale_adaptation
            im_patch_scale = getScaleSubwindow(im, pos, base_target_sz, scale_factor*scale_factors, scale_window, scale_model_sz, p.hog_scale_cell_size);
            xsf = fft(im_patch_scale,[],2);
            new_sf_num = bsxfun(@times, ysf, conj(xsf));
            new_sf_den = sum(xsf .* conj(xsf), 1);
            if frame == 1,
                sf_den = new_sf_den;
                sf_num = new_sf_num;
            else
                sf_den = (1 - p.learning_rate_scale) * sf_den + p.learning_rate_scale * new_sf_den;
                sf_num = (1 - p.learning_rate_scale) * sf_num + p.learning_rate_scale * new_sf_num;
            end
        end

        % update bbox position
        if frame==1, rect_position = [pos([2,1]) - target_sz([2,1])/2, target_sz([2,1])]; end

        rect_position_padded = [pos([2,1]) - bg_area([2,1])/2, bg_area([2,1])];

        OTB_rect_positions(frame,:) = rect_position;

        if p.fout > 0,  fprintf(p.fout,'%.2f,%.2f,%.2f,%.2f\n', rect_position(1),rect_position(2),rect_position(3),rect_position(4));   end

        elapsed_time = elapsed_time + toc();

        %% VISUALIZATION
        if p.visualization == 1
            if isToolboxAvailable('Computer Vision System Toolbox')
                im = insertShape(im, 'Rectangle', rect_position, 'LineWidth', 4, 'Color', 'black');
                im = insertShape(im, 'Rectangle', rect_position_padded, 'LineWidth', 4, 'Color', 'yellow');
                % Display the annotated video frame using the video player object.
                step(p.videoPlayer, im);
            else
                figure(1)
                imshow(im)
                rectangle('Position',rect_position, 'LineWidth',2, 'EdgeColor','g');
                rectangle('Position',rect_position_padded, 'LineWidth',2, 'LineStyle','--', 'EdgeColor','b');
                drawnow
            end
        end
    end

    % save data for OTB-13 benchmark
    results.type = 'rect';
    results.res = OTB_rect_positions;
    results.fps = num_frames/(elapsed_time );
end

% Reimplementation of Hann window (in case signal processing toolbox is missing)
function H = myHann(X)
    H = .5*(1 - cos(2*pi*(0:X-1)'/(X-1)));
end

% We want odd regions so that the central pixel can be exact
function y = floor_odd(x)
    y = 2*floor((x-1) / 2) + 1;
end

function y = ensure_real(x)
    assert(norm(imag(x(:))) <= 1e-5 * norm(real(x(:))));
    y = real(x);
end


%% ArUco Grid Board Image Demo
% This example shows how to create an ArUco grid board image.
%
% Sources:
%
% * <https://github.com/opencv/opencv_contrib/blob/3.1.0/modules/aruco/samples/create_board.cpp>
%

%% Board

% params
markersX = 5;                % Number of markers in X direction
markersY = 7;                % Number of markers in Y direction
markerLength = 60;           % Marker side length (in pixels)
markerSeparation = 15;       % Separation between two consecutive markers in the grid (in pixels)
dictionaryId = '6x6_250';    % dictionary id
margins = markerSeparation;  % Margins size (in pixels)
borderBits = 1;              % Number of bits in marker borders

imageSize = [markersY, markersX] .* ...
    (markerLength + markerSeparation) - markerSeparation + 2 * margins;

% create board
dictionary = {'Predefined', dictionaryId};
board = {'GridBoard', ...
    markersX, markersY, markerLength, markerSeparation, dictionary};

%% Display

% show created board
boardImage = cv.drawPlanarBoard(board, fliplr(imageSize), ...
    'MarginSize',margins, 'BorderBits',borderBits);
imshow(boardImage), title('GridBoard')

% save image
imwrite(boardImage, fullfile(tempdir(),'GridBoard.png'));


function angles = wrapMinusPiPifast(angles)

tol = 0.001;

angles(angles < -pi) = angles(angles < -pi) + 2*pi;
angles(angles < -pi) = angles(angles < -pi) + 2*pi;

angles(angles > pi) = angles(angles > pi) - 2*pi;
angles(angles > pi) = angles(angles > pi) - 2*pi;

angles(angles == pi) = angles(angles == pi) - tol;
angles(angles == -pi) = angles(angles == -pi) + tol;

assert(~any(angles > pi));
assert(~any(angles < -pi));

end