classdef PTKVesselness < PTKPlugin
    % PTKVesselness. Plugin for detecting blood vessels
    %
    %     This is a plugin for the Pulmonary Toolkit. Plugins can be run using
    %     the gui, or through the interfaces provided by the Pulmonary Toolkit.
    %     See PTKPlugin.m for more information on how to run plugins.
    %
    %     Plugins should not be run directly from your code.
    %
    %     PTKVesselness computes a mutiscale vesselness filter based on Frangi et
    %     al., 1998. "Multiscale Vessel Enhancement Filtering". The filter
    %     returns a value at each point which in some sense representes the
    %     probability of that point belonging to a blood vessel.
    %
    %     To reduce memory usage, the left and right lungs are filtered
    %     separately and each is further divided into subimages using the
    %     PTKImageDividerHessian function. This will compute the eigenvalues of
    %     the Hessian matrix for each subimage and use these to compute the
    %     vesselness using the PTKComputeVesselnessFromHessianeigenvalues
    %     function.
    %
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    properties
        ButtonText = 'Vesselness'
        ToolTip = 'Shows the multiscale vesselness filter for detecting blood vessels'
        Category = 'Vessels'
        AllowResultsToBeCached = true
        AlwaysRunPlugin = false
        PluginType = 'ReplaceOverlay'
        HidePluginInDisplay = false
        FlattenPreviewImage = false
        PTKVersion = '1'
        ButtonWidth = 6
        ButtonHeight = 2
        GeneratePreview = true
        Visibility = 'Developer'
    end

    methods (Static)

        function results = RunPlugin(dataset, reporting)

            right_lung = dataset.GetResult('PTKGetRightLungROI');

            reporting.PushProgress;

            reporting.UpdateProgressStage(0, 2);
            vesselness_right = PTKVesselness.ComputeVesselness(right_lung, reporting, false);

            reporting.UpdateProgressStage(1, 2);
            left_lung = dataset.GetResult('PTKGetLeftLungROI');
            vesselness_left = PTKVesselness.ComputeVesselness(left_lung, reporting, true);

            reporting.PopProgress;

            left_and_right_lungs = dataset.GetResult('PTKLeftAndRightLungs');

            results = PTKCombineLeftAndRightImages(dataset.GetTemplateImage(PTKContext.LungROI), vesselness_left, vesselness_right, left_and_right_lungs);

            lung = dataset.GetResult('PTKLeftAndRightLungs');
            results.ChangeRawImage(results.RawImage.*single(lung.RawImage > 0));
            results.ImageType = PTKImageType.Scaled;
        end

        function results = GenerateImageFromResults(results, ~, ~)
            vesselness_raw = 3*uint8(results.RawImage > 5);
            results.ChangeRawImage(vesselness_raw);
            results.ImageType = PTKImageType.Colormap;
        end

    end

    methods (Static, Access = private)

        function vesselness = ComputeVesselness(image_data, reporting, is_left_lung)

            reporting.PushProgress;

            sigma_range = 0.5 : 0.5: 2;
            num_calculations = numel(sigma_range);
            vesselness = [];
            progress_index = 0;
            for sigma = sigma_range
                reporting.UpdateProgressStage(progress_index, num_calculations);
                progress_index = progress_index + 1;

                mask = [];
                vesselness_next = PTKImageDividerHessian(image_data.Copy, @PTKVesselness.ComputeVesselnessPartImage, mask, sigma, [], false, false, is_left_lung, reporting);
                vesselness_next.ChangeRawImage(100*vesselness_next.RawImage);
                if isempty(vesselness)
                    vesselness =  vesselness_next.Copy;
                else
                    vesselness.ChangeRawImage(max(vesselness.RawImage, vesselness_next.RawImage));
                end
            end

            reporting.PopProgress;

        end

        function vesselness_wrapper = ComputeVesselnessPartImage(hessian_eigs_wrapper, voxel_size)
            vesselness_wrapper = PTKComputeVesselnessFromHessianeigenvalues(hessian_eigs_wrapper, voxel_size);
        end

    end
end


function p02_story ( )

%*****************************************************************************80
%
%% P02_STORY prints the "story" for problem p02.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    ETY Lee,
%    Choosing Nodes in Parametric Curve Interpolation,
%    Computer-Aided Design,
%    Volume 21, Number 6, July/August 1989, pages 363-370.
%
%  Parameters:
%
%    None
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  This example is due to ETY Lee of Boeing.\n' );
  fprintf ( 1, '  Data near the corners is more dense than in regions of small curvature.\n' );
  fprintf ( 1, '  A local interpolation method will produce a more plausible\n' );
  fprintf ( 1, '  interpolant than a nonlocal interpolation method, such as\n' );
  fprintf ( 1, '  cubic splines.\n' );

  return
end


function [quality probs] = biqi(im)

%========================================================================
%
% -----------COPYRIGHT NOTICE STARTS WITH THIS LINE------------
% Copyright (c) 2009 The University of Texas at Austin
% All rights reserved.
%
% Permission is hereby granted, without written agreement and without license or royalty fees, to use, copy,
% modify, and distribute this code (the source files) and its documentation for
% any purpose, provided that the copyright notice in its entirety appear in all copies of this code, and the
% original source of this code, Laboratory for Image and Video Engineering (LIVE, http://live.ece.utexas.edu)
% and Center for Perceptual Systems (CPS, http://www.cps.utexas.edu) at the University of Texas at Austin (UT Austin,
% http://www.utexas.edu), is acknowledged in any publication that reports research using this code. The research
% is to be cited in the bibliography as:
%
% 1. A. K. Moorthy and A. C. Bovik, "A Modular Framework for Constructing Blind
% Universal Quality Indices", submitted to IEEE Signal Processing Letters (2009).
%
% 2. A. K. Moorthy and A. C. Bovik, "BIQI Software Release",
% URL: http://live.ece.utexas.edu/research/quality/biqi.zip, 2009.
%
% IN NO EVENT SHALL THE UNIVERSITY OF TEXAS AT AUSTIN BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL,
% OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OF THIS DATABASE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF TEXAS
% AT AUSTIN HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%
% THE UNIVERSITY OF TEXAS AT AUSTIN SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
% WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE DATABASE PROVIDED HEREUNDER IS ON AN "AS IS" BASIS,
% AND THE UNIVERSITY OF TEXAS AT AUSTIN HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
%
% -----------COPYRIGHT NOTICE ENDS WITH THIS LINE------------%

%Author  : Anush Krishna Moorthy
%Version : 1.0
%
%The authors are with the Laboratory for Image and Video Engineering
%(LIVE), Department of Electrical and Computer Engineering, The
%University of Texas at Austin, Austin, TX.
%
%Kindly report any suggestions or corrections to anushmoorthy@gmail.com
%
%========================================================================
%
% This is a demonstration of the Blind Image Quality Index (BIQI) .
% It is an implementation of the BIQI in the reference.
% The algorithm is described in:
% A. K. Moorthy and A. C. Bovik, "A Modular Framework for Constructing Blind
% Universal Quality Indices", submitted to IEEE Signal Processing Letters (2009).
%
%You can change this program as you like and use it anywhere, but please
%refer to its original source (cite our paper and our web page at
% http://live.ece.utexas.edu/research/quality/biqi.zip).
%
%Input : A test 8bits/pixel grayscale image loaded in a 2-D array
%Output: A quality score of the image. The score typically has a value
%        between 0 and 100 (0 represents the best quality, 100 the worst).
%
%Usage:
%
%1. Load the image, for example
%
%   image = rgb2gray(imread('testimage.jpg'));
%
%2. Call this function to calculate the quality score:
%
%    quality = biqi(image)
%
% Dependencies:
% MATLAB Wavelet  Toolbox
% You may need the MATLAB Image Processing Toolbox
% Binaries: svm-train, svm-scale (from LibSVM) - provided with release
% Other m files: jpeg_quality_score.m (provided with release)
% Data files: range2, range2_wn, range2_blur, range2_jp2k, model_89,
% model_89_wn, model_89_blur, model_89_jp2k, rang2_ff model_ff
%========================================================================

import biqi.*

if(size(im,3)~=1)
    im = rgb2gray(im);
end

%% First compute statistics
num_scales = 3; % somethings are hardcoded for this...please be careful when changing.
gam = 0.2:0.001:10;
r_gam = gamma(1./gam).*gamma(3./gam)./(gamma(2./gam)).^2;

[C S] = wavedec2(im,num_scales,'db9');
for p = 1:num_scales
    [horz_temp,vert_temp,diag_temp] = detcoef2('all',C,S,p) ;
    horz(p) = {[horz_temp(:)]};
    diag(p) = {[diag_temp(:)]};
    vert(p) = {[vert_temp(:)]};

    h_horz_curr  = cell2mat(horz(p));
    h_vert_curr  = cell2mat(vert(p));
    h_diag_curr  = cell2mat(diag(p));

    mu_horz(p) = mean(h_horz_curr);
    sigma_sq_horz(p)  = mean((h_horz_curr-mu_horz(p)).^2);
    E_horz = mean(abs(h_horz_curr-mu_horz(p)));
    rho_horz = sigma_sq_horz(p)/E_horz^2;
    [min_difference, array_position] = min(abs(rho_horz - r_gam));
    gam_horz(p) = gam(array_position);

    mu_vert(p) = mean(h_vert_curr);
    sigma_sq_vert(p)  = mean((h_vert_curr-mu_vert(p)).^2);
    E_vert = mean(abs(h_vert_curr-mu_vert(p)));
    rho_vert = sigma_sq_vert(p)/E_vert^2;
    [min_difference, array_position] = min(abs(rho_vert - r_gam));
    gam_vert(p) = gam(array_position);

    mu_diag(p) = mean(h_diag_curr);
    sigma_sq_diag(p)  = mean((h_diag_curr-mu_diag(p)).^2);
    E_diag = mean(abs(h_diag_curr-mu_diag(p)));
    rho_diag = sigma_sq_diag(p)/E_diag^2;
    [min_difference, array_position] = min(abs(rho_diag - r_gam));
    gam_diag(p) = gam(array_position);
end
rep_vec = [mu_horz mu_vert mu_diag sigma_sq_horz sigma_sq_vert sigma_sq_diag gam_horz gam_vert gam_diag];
rep_vec(:,1:9) = []; % remove the means...
%% Now classify

fid = fopen('test_ind.txt','w')
for j = 1:size(rep_vec,1)
    fprintf(fid,'%d ',j);
    for k = 1:size(rep_vec,2)
        fprintf(fid,'%d:%f ',k,rep_vec(j,k));
    end
    fprintf(fid,'\n');
end
fclose(fid);

system(['/usr/local/Cellar/libsvm/3.22/bin/svm-scale -r +biqi/range2 test_ind.txt >> test_ind_scaled']);
system(['/usr/local/Cellar/libsvm/3.22/bin/svm-predict -b 1 test_ind_scaled +biqi/model_89 output_89']);
delete test_ind.txt test_ind_scaled

%% Quality along each dimension

% Write out SVM compatible

fid = fopen('test_ind.txt','w');
for j = 1:size(rep_vec,1)
    fprintf(fid,'%f ',j);
    for k = 1:size(rep_vec,2)
        fprintf(fid,'%d:%f ',k,rep_vec(j,k));
    end
    fprintf(fid,'\n');
end
fclose(fid);

% Jp2k quality
system(['/usr/local/Cellar/libsvm/3.22/bin/svm-scale -r +biqi/range2_jp2k test_ind.txt >> test_ind_scaled']);
system(['/usr/local/Cellar/libsvm/3.22/bin/svm-predict  -b 1 test_ind_scaled +biqi/model_89_jp2k output_blur']);
load output_blur
jp2k_score = output_blur;
delete output_blur test_ind_scaled

% JPEG quality
jpeg_score  = jpeg_quality_score(im);

% WN quality
system(['/usr/local/Cellar/libsvm/3.22/bin/svm-scale -r +biqi/range2_wn test_ind.txt >> test_ind_scaled']);
system(['/usr/local/Cellar/libsvm/3.22/bin/svm-predict -b 1 test_ind_scaled +biqi/model_89_wn output_blur']);
load output_blur
wn_score = output_blur;
delete output_blur test_ind_scaled

% Blur quality
system(['/usr/local/Cellar/libsvm/3.22/bin/svm-scale -r +biqi/range2_blur test_ind.txt >> test_ind_scaled']);
system(['/usr/local/Cellar/libsvm/3.22/bin/svm-predict  -b 1 test_ind_scaled +biqi/model_89_blur output_blur']);
load output_blur
blur_score = output_blur;
delete output_blur test_ind_scaled

% FF quality
system(['/usr/local/Cellar/libsvm/3.22/bin/svm-scale -r +biqi/range2_ff test_ind.txt >> test_ind_scaled']);
system(['/usr/local/Cellar/libsvm/3.22/bin/svm-predict  -b 1 test_ind_scaled +biqi/model_89_ff output_blur']);
load output_blur
ff_score = output_blur;

delete output_blur
delete test_ind.txt test_ind_scaled

%% Final pooling

% figure out probabilities
fid = fopen('output_89','r');
fgetl(fid);
C = textscan(fid,'%f %f %f %f %f %f');
output = [C{1} C{2} C{3} C{4} C{5} C{6}];
fclose(fid);
probs = output(:,2:end);
scores  = [jp2k_score jpeg_score wn_score blur_score ff_score];
quality = sum(probs.*scores,2);
delete output_89
clc


function cx = zscal ( n, ca, cx, incx )

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

  return
end


% THIS SOFTWARE AND ANY ACCOMPANYING DOCUMENTATION IS RELEASED "AS IS."  THE U.S. GOVERNMENT MAKES NO WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, CONCERNING THIS SOFTWARE AND ANY ACCOMPANYING DOCUMENTATION, INCLUDING, WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE.  IN NO EVENT WILL THE U.S. GOVERNMENT BE LIABLE FOR ANY DAMAGES, INCLUDING ANY LOST PROFITS, LOST SAVINGS OR OTHER INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE, OR INABILITY TO USE, THIS SOFTWARE OR ANY ACCOMPANYING DOCUMENTATION, EVEN IF INFORMED IN ADVANCE OF THE POSSIBILITY OF SUCH DAMAGES.
%
% file: prop_diff_ci.m

% 030110 tdr created
% 031223 tdr added test for NaN in inputs
% 040203 tdr corrected error found by Steve Walls in check_inputs (was using n in place of n1 when ensuring that we had integers)

function metrics = prop_diff_ci(x1,n1,x2,n2,alpha,method,verbose)

if (nargin ~= 7 & nargin ~= 6 & nargin ~= 5),
    error('Requires 5, 6, or 7 input arguments (x1,n1,x2,n2,alpha), (x1,n1,x2,n2,alpha,method), or (x1,n1,x2,n2,alpha,method,verbose); default method = 3');
end
if nargin == 5, method = 3; verbose = 0; end;
if nargin == 6, verbose = 0; end;
inputsOkay = check_inputs(x1,n1,x2,n2,alpha,method,verbose);
if ~inputsOkay
    metrics = [NaN, NaN, NaN];
    return
end;

tic;

% call selected interval estimator method
switch method
case 1, % one-sided CI - note provides both upper and lower one-sided CIs
    ci = get_prop_diff_ci1(x1,n1,x2,n2,alpha,method,verbose);
case 2, % minimum length CI
    error('Method 2 Not Available - choose Method 1, 3, 4, 5 or 6');
case 3, % symmetrical about estimate, expect when outside range
    ci = get_prop_diff_ci3(x1,n1,x2,n2,alpha,method,verbose);
case 4, % equal tail masses, note - may not enclose estimate
    ci = get_prop_diff_ci1(x1,n1,x2,n2,alpha/2,method,verbose);
case 5, % Using Fagan's application of "exact" method
    ci = get_ci_fagan(x1,n1,x2,n2,alpha,method,verbose);
case 6, % Normal Approximation
    ci = get_ci_rss(x1,n1,x2,n2,alpha,method,verbose);
case 7, % Law of Large Numbers Approximation
    error('Method 7 Not Available - choose Method 1, 3, 4, 5 or 6');
otherwise
   error('Not a valid method: 1 (1-sided), 2 (min length), 3 (symm.), 4 (equal tail), 5 (MatLab-binofit), 6 (Normal Approx)');
end;
rt = toc;

if verbose
    p1_hat = x1/n1; p2_hat = x2/n2; delta_p_hat = p1_hat - p2_hat;
    length = ci(3)-ci(2);
    lower_tail = 1 - prop_diff(x1,n1,x2,n2,ci(2));
    upper_tail = prop_diff(x1,n1,x2,n2,ci(3));
    if (method == 1)
        actual_alpha = (lower_tail + upper_tail)/2;
    else
        actual_alpha = lower_tail + upper_tail;
    end;
    if (abs(actual_alpha - alpha) > 0.00005) % close_enough is 0.00001 throughout, but spec is 0.0005.
        warning('Interval not to spec: abs(desired_alpha - actual_alpha) < 0.00005)');
        warning_stats = [alpha actual_alpha alpha-actual_alpha]
    end;
    disp('p_diff_hat, Lower CI Bound, Upper CI Bound, x1, n1, x2, n2, Desired alpha, Method, Length, Lower Tail, Upper Tail, Actual alpha, Delta alpha, Run Time')
    metrics = [ci x1 n1 x2 n2 alpha method length lower_tail upper_tail actual_alpha (alpha - actual_alpha) rt];
    disp(sprintf('%8.6g, %8.6g, %8.6g, %d, %d, %d, %d, %8.6g, %d, %8.6g, %8.6g, %8.6g, %8.6g, %8.6g, %8.2g',metrics));
else
    metrics = ci;
end;

% ------------------------------------------------------------
% Difference CI based on root sum of squares of individual CI's, see
% Fagan'99 in Computers in Biology and Medicine.

function  ci = get_ci_fagan(x1,n1,x2,n2,alpha,method,verbose);
    ci1 = prop_ci(x1,n1,alpha,method);
    ci2 = prop_ci(x2,n2,alpha,method);
    diff = ci1(1) - ci2(1);
    correction_factor = 0.18 / max(n1,n2); % see Fagan'99, p.85
    rss_lower = diff - sqrt((ci1(2)-ci1(1))^2 + (ci2(3)-ci2(1))^2);
    rss_upper = diff + sqrt((ci1(3)-ci1(1))^2 + (ci2(2)-ci2(1))^2);
    diff_lower = max((ci1(2)-ci2(3)), (rss_lower - correction_factor));
    diff_upper = min((ci1(3)-ci2(2)), (rss_upper + correction_factor));
    ci = [diff diff_lower diff_upper];

% ------------------------------------------------------------
% Difference CI based on root sum of squares of individual CI's

function  ci = get_ci_rss(x1,n1,x2,n2,alpha,method,verbose);
    ci1 = prop_ci(x1,n1,alpha,method);
    ci2 = prop_ci(x2,n2,alpha,method);
    diff = ci1(1) - ci2(1);
    rss_lower = diff - sqrt((ci1(2)-ci1(1))^2 + (ci2(3)-ci2(1))^2);
    rss_upper = diff + sqrt((ci1(3)-ci1(1))^2 + (ci2(2)-ci2(1))^2);
    ci = [diff rss_lower rss_upper];

% ------------------------------------------------------------
% start checking inputs
function inputsOkay = check_inputs(x1,n1,x2,n2,alpha,method,verbose)

inputsOkay = 1;

% inputs must all be scalars
if (max(max([size(x1); size(n1); size(x2); size(n2); size(alpha)])) ~= 1)
    error('Non-scalar input');
end;

if isnan(x1) | isnan(n1) | isnan(x2) | isnan(n2) | isnan(alpha)
    warning('NaN input')
    inputsOkay = 0;
    return
end;

% x and n must be integers
if (round(x1) ~= x1)
    x1 = round(x1);
    warning('Non-integer input x1');
end;
if (round(x2) ~= x2)
    x2 = round(x2);
    warning('Non-integer input x2');
end;
if (round(n1) ~= n1)
    n1 = round(n1);
    warning('Non-integer input n1');
end;
if (round(n2) ~= n2)
    n2 = round(n2);
    warning('Non-integer input n2');
end;

% n must be > 0
if (n1 <= 0 | n2 <= 0),
   inputsOkay = 0;
   warning('n <= 0');
   return;
end;

% n should be < 10^6
if (n1 > 10^6 | n2 > 10^6),
   warning('n > 10^6, Results may not be accurate.');
end;

% x must be >= 0 and <=n
if ( x1 < 0 | x1 > n1 | x2 < 0 | x2 > n2),
   inputsOkay = 0;
   warning('x < 0 or x > n');
   return;
end;

% alpha must be > 0.0 and < 1.0
if ( alpha < 0 | alpha > 1),
   inputsOkay = 0;
   warning('alpha < 0 or alpha > 1');
   return;
end;

% verbose must be 0 or 1
if ( verbose ~= 0 & verbose ~= 1),
   inputsOkay = 0;
   warning('verbose not zero or one');
   return;
end;
% end checking inputs
% ------------------------------------------------------------


function [anx,any,anz,dx,dy,dz,ierr] = focal_pt2nd(wpx,wpy,wpz,wtx,wty,wtz)

    % compute Cartesian component of P and T versors from outward normal and slip vectors
    %
    %     usage:
    %     call pt2nd(px,py,pz,tx,ty,tz,anx,any,anz,dx,dy,dz,ierr)
    %
    %     arguments:
    %     px,py,pz       components of P (maximum dilatation) axis vector
    %                    in the Aki-Richards Cartesian coordinate system (INPUT)
    %     tx,ty,tz       components of T (maximum tension) axis vector
    %                    in the Aki-Richards Cartesian coordinate system (INPUT)
    %     anx,any,anz    components of fault plane outward normal versor in the
    %                    Aki-Richards Cartesian coordinate system (OUTPUT)
    %     dx,dy,dz       components of slip versor in the Aki-Richards
    %                    Cartesian coordinate system (OUTPUT)
    %     ierr           error indicator (OUTPUT)
    %
    %     errors:
    %     1              input vectors not perpendicular among each other
    %
    %      call fpsset
    amistr=-360.;
    amastr=360.;
    amidip=0.;
    amadip=90.;
    amirak=-360.;
    amarak=360.;
    amitre=-360.;
    amatre=360.;
    amiplu=0.;
    amaplu=90.;
    orttol=2.;
    ovrtol=0.001;
    tentol=0.0001;
    dtor=0.017453292519943296;
    c360=360.;
    c90=90.;
    c0=0.;
    c1=1.;
    c2=2.;
    c3=3.;

    %%c
    anx=c0;
    any=c0;
    anz=c0;
    dx=c0;
    dy=c0;
    dz=c0;
    ierr=0;
    [ang] = focal_angle(wpx,wpy,wpz,wtx,wty,wtz);
    if (abs(ang - c90) > orttol)
        disp(['PT2ND: input vectors not perpendicular, angle=' num2str(ang)]);
        ierr = 1;
    end
    [pnorm,px,py,pz] = focal_norm(wpx,wpy,wpz);

    if (pz < c0)
        [px,py,pz] = focal_invert(px,py,pz);
    end
    [tnorm,tx,ty,tz] = focal_norm(wtx,wty,wtz);
    if (tz < c0)
        [tx,ty,tz] = focal_invert(tx,ty,tz);
    end
    anx = tx + px;
    any = ty + py;
    anz = tz + pz;
    [amn,anx,any,anz] = focal_norm(anx,any,anz);
    % c
    dx = tx - px;
    dy = ty - py;
    dz = tz - pz;
    [amn,dx,dy,dz] = focal_norm(dx,dy,dz);
    if(anz > c0)
        [anx,any,anz] = focal_invert(anx,any,anz);
        [dx,dy,dz] = focal_invert(dx,dy,dz);
    end
end


classdef quadcopter < agent
    properties
        % GLOBAL.position   - The position of the object in the global axes
        % GLOBAL.velocity   - The velocity of the object in the global axes
        % GLOBAL.quaternion - The quaternion representing the earth to rotated body

        % DYNAMICS - All the models parameters are held in the DYNAMICS
        %            container field.
    end
    %% ////////////////////////// MAIN METHODS /////////////////////////////
    methods
        % Constructor
        function [this] = quadcopter(varargin)
            % Call the super class
            this@agent(varargin);

            % Create the dynamics of a quadcopter
            this.DYNAMICS = this.CreateDYNAMICS();

            % //////////////// Check for user overrides ///////////////////
            this = this.ApplyUserOverrides(varargin); % Recursive overrides
            % /////////////////////////////////////////////////////////////
        end
        % Setup
        function [this] = setup(this,localXYZVelocity,localXYZrotations)
            % This function calculates the intial state for a quadrotor
            % object.

            % For a state vector defined as x_k:
            % [x,y,z,dx,dy,dz,R11,R12,R13,R21,R22,R23,R31,R32,R33,wx,wy,wz]

            % THIS MODEL OPERATES IN THE GLOBAL FRAME
            p0 = this.GetGLOBAL('position');   % True global position
            v0 = this.GetGLOBAL('velocity');   % True global velocity
            % GET THE ROTATION MATRIX fixed local axes -> rotated global pose
            R0 = OMAS_geometry.eulersToRotationMatrix(localXYZrotations);
            % Build an initial (global) state vector
            vecR0   = reshape(R0',9,1);        % Defines local vector to global vector
            omega0  = zeros(3,1);
            x0      = [p0;v0;vecR0;omega0];
            % ASSIGN THE LOCAL FRD STATE
            this.SetGLOBAL('priorState',x0);
            this.localState = x0;
        end
        % Main
        function [this] = main(this,ENV,varargin)

            % //////////// CHECK FOR NEW INFORMATION UPDATE ///////////////
            % Update the agent with the environmental data
            [this,~,~] = this.GetAgentUpdate(ENV,varargin{1});
            % /////////////////////////////////////////////////////////////

            % The state reference
            desiredPosition = [0;0;0];

            % Call the controller loop
            this = this.Controller(ENV,desiredPosition);

            % /////////////// RECORD THE AGENT-SIDE DATA //////////////////
            this.DATA.inputNames = {'$\dot{x}$ (m/s)','$\dot{y}$ (m/s)','$\dot{z}$ (m/s)',...
                '$p$ (rad/s)','$q$ (rad/s)','$r$ (rad/s)'};
            this.DATA.inputs(1:length(this.DATA.inputNames),ENV.currentStep) = [this.localState(4:6);this.localState(16:end)];          % Record the control inputs
        end
    end
    %% //////////////////////// AUXILLARY METHODS //////////////////////////
    methods
        % The quadcopter controller
        function [this] = Controller(this,ENV,y_k)

            % Input sanity checks
            assert(IsColumn(y_k,3),"Expecting a numeric velocity vector.");
            assert(1 == size(this.localState,2),"The length of the objects state update must match the its local state.");

            % ///////////// UPDATE THE LOCAL STATE VECTOR /////////////////
            x_k_plus = this.UpdateLocalState(ENV,this.localState,y_k);
            % Update the global properties
            this = this.GlobalUpdate(x_k_plus);
        end
        % Get the state update (using ode45)
        function [x_k_plus] = UpdateLocalState(this,ENV,x_k,y_desired)
            % This function computes the state update for the current agent
            % using the ode45 function.

            % Integrate across the time delta "dt"
            opts = odeset('RelTol',1e-2,'AbsTol',ENV.dt*1E-1);

            [~,Xset] = ode45(@(t,X) this.ClosedLoopDynamics_position(X,y_desired),[0 ENV.dt],x_k,opts);
%             [~,Xset] = ode45(@(t,X) this.ClosedLoopDynamics_velocity(X,y_desired),[0 ENV.dt],x_k,opts);

            % Assign the integral state
            x_k_plus = Xset(end,:)';
        end
        % Global update from the new state
        function [this] = GlobalUpdate(this,x_k_plus)
            % This function updates the global structure from the new state
            % definition: [p_k;v_k;vecR_k;omega_k]

            % Extract the rotation matrix components
            R_k_plus = reshape(x_k_plus(7:15),3,3);
            % Define the new quaternion from R
            q_k_plus = OMAS_geometry.rotationMatrixToQuaternion(R_k_plus');

            % //////////////// UPDATE GLOBAL PROPERTIES ///////////////////
            [this] = this.GlobalUpdate_direct(...
                x_k_plus(1:3),...   % The global position is within the state
                x_k_plus(4:6),...   % The global velocity is within the state
                q_k_plus);          % Append the global quaternion

            % Ensure the local state is re-assigned
            this.localState = x_k_plus;
        end
    end
    % Dynamic functions
    methods
        % Quadcopter Dynamics (Closed-loop)
        function [dxdt] = ClosedLoopDynamics_velocity(this,x_k,v_desired)

            % Extract the current state properties
            p_k     = x_k(1:3);
            v_k     = x_k(4:6);
            R_k     = reshape(x_k(7:15),3,3); % The rotation matrix components
            eta_k   = OMAS_geometry.rotationMatrixToEulers(R_k);
            omega_k = x_k(16:18);

            % The state reference
            y_k = [p_k;v_k;eta_k;omega_k];

            % Extract the parameters from the state
            psi_desired = pi/2;
            y_desired = [zeros(3,1);v_desired;[0;0;psi_desired];zeros(3,1)];

            % Extract DYNAMIC parameters
            g  = this.DYNAMICS.g;
            m  = this.DYNAMICS.m;

            % State matrix
            A = this.DYNAMICS.A;
            A(4:6,7:9) = g*[ sin(psi_desired), cos(psi_desired), 0;
                            -cos(psi_desired), sin(psi_desired), 0;
                                            0,                0, 0];
            % Input matrix
            B = this.DYNAMICS.B;

            % LQR control gain
            [K,~,~] = lqr(A,B,eye(12),eye(4));
            K(:,1:3) = 0;
            % Calculate the error
            e_k  = y_k - y_desired;
            du_k = -K*e_k;
            % Calculate the inputs
            u_k = [m*g;0;0;0] + du_k;

            % Provide the inputs to the open-loop dynamics
            [dxdt] = this.OpenLoopDynamics(x_k,u_k);
        end
        % Quadcopter Dynamics (Closed-loop)
        function [dxdt] = ClosedLoopDynamics_position(this,x_k,p_desired)

            % Extract the current state properties
            p_k     = x_k(1:3);
            v_k     = x_k(4:6);
            R_k     = reshape(x_k(7:15),3,3); % The rotation matrix components
            eta_k   = OMAS_geometry.rotationMatrixToEulers(R_k);
            omega_k = x_k(16:18);

            % The state reference
            y_k = [p_k;v_k;eta_k;omega_k];

            % Extract the parameters from the state
            psi_desired = 0;
            y_desired = [p_desired(1:3);zeros(3,1);[0;0;psi_desired];zeros(3,1)];

            % Extract DYNAMIC parameters
            e3 = this.DYNAMICS.e3;
            g  = this.DYNAMICS.g;
            m  = this.DYNAMICS.m;
            I  = this.DYNAMICS.I;

            % State matrix
            A = this.DYNAMICS.A;
            A(1:3,4:6) = eye(3);
            A(4:6,7:9) = g*[ sin(psi_desired), cos(psi_desired), 0;
                -cos(psi_desired), sin(psi_desired), 0;
                0,                0, 0];
            A(7:9,10:12)=eye(3);

            % Input matrix
            B = this.DYNAMICS.B;
            B(4:6,1) = e3/m;
            B(10:12,2:4) = inv(I);

            % LQR control gain
            [K,~,~] = lqr(A,B,eye(12),eye(4));
            % Calculate the error
            e_k  = y_k - y_desired;
            du_k = -K*e_k;
            % Calculate the inputs
            u_k = [m*g;0;0;0] + du_k;

            % Provide the inputs to the open-loop dynamics
            [dxdt] = this.OpenLoopDynamics(x_k,u_k);
        end
        % Quadcopter Dynamics (Open-loop)
        function [dxdt] = OpenLoopDynamics(this,x_k,u_k)

            % Extract the state parameters
            v_k = x_k(4:6);
            R_k = reshape(x_k(7:15),3,3);
            w_k = x_k(16:18);
            % Extract the inputs
            f   = u_k(1);
            tau = u_k(2:4);

            % Get the constants
            m  = this.DYNAMICS.m;
            g  = this.DYNAMICS.g;
            I  = this.DYNAMICS.I;
            e3 = this.DYNAMICS.e3;

            % nonlinear dynamics based on rotation dynamics
            p_dot   = v_k;
            v_dot   = f/m*R_k*e3-g*e3;
            R_dot    = R_k*skew(w_k);
            vecR_dot = reshape(R_dot,9,1);
            w_dot    = inv(I)*(tau-skew(w_k)*I*w_k);

            % THE STATE DIFFERENCE
            dxdt = [p_dot;v_dot;vecR_dot;w_dot];
        end
    end
    % Dynamics container
    methods
        % Get the (generic) quadcopter dynamic properties
        function [DYNAMICS] = CreateDYNAMICS(this)
            % Simple quadcopter, use the default properties
            DYNAMICS = this.CreateDYNAMICS_default();
        end
        % Create (quadcopter) default dynamic structure
        function [DYNAMICS] = CreateDYNAMICS_default(this)
            % Define an infinite "throw" range by default
            DYNAMICS = struct();
            % Define kinematic constraints
            DYNAMICS.maxLinearVelocity      = ones(3,1)*this.v_max;	% Limits on the agents linear velocity
            DYNAMICS.maxLinearAcceleration  = inf(3,1);            	% Limits on the agents linear acceleration
            DYNAMICS.maxAngularVelocity     = ones(3,1)*this.w_max;	% Limits on the agents angular velocity
            DYNAMICS.maxAngularAcceleration = inf(3,1);            	% Limits on the agents angular acceleration
            % Dynamical properties
            DYNAMICS.e3 = [0;0;1];                                   % Local z-axis
            DYNAMICS.g  = 300978377846937375/30680772461461504;      % Gravitational constant
            DYNAMICS.m  = 1;                                         % Quadcopter mass
            DYNAMICS.I  = 2*diag([4.856, 4.856, 8.801])*10^(-3);     % Quadcopter inertia
            % Control properties
            % Plant matrix
            DYNAMICS.A = zeros(12);
            DYNAMICS.A(4:6,7:9)   = eye(3)*DYNAMICS.g;
            DYNAMICS.A(1:3,4:6)   = eye(3);
            DYNAMICS.A(7:9,10:12) = eye(3);
            % Input matrix
            DYNAMICS.B = zeros(12,4);
            DYNAMICS.B(4:6,1)     = DYNAMICS.e3/DYNAMICS.m;
            DYNAMICS.B(10:12,2:4) = inv(DYNAMICS.I);
            % Observation matrix
            DYNAMICS.C = eye(12);
            % Feed-forward matrix
            DYNAMICS.D = eye(4);
        end
    end
end


function out = iszero(f)
%ISZERO   Check if a CHEBFUN3 is identically zero on its domain.
%   OUT = ISZERO(F) returns logical 1 if the CHEBFUN3 object F is exactly
%   the zero function, and logical 0 otherwise.
%
% See also CHEBFUN2/ISZERO.

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

% Get data:
[fCore, fCols, fRows, fTubes] = tucker(f);

% Trivial check: If all the pivots are zero, then the SEPARABLEAPPROX is zero:
if ( norm(fCore(:), inf) == 0 )
    out = 1;
    return
end

% Quick check: Evaluate on a grid. If the tensor is nonzero then the
% CHEBFUN3 is nonzero.
dom = f.domain;
x = linspace(dom(1), dom(2), 10);
y = linspace(dom(3), dom(4), 10);
z = linspace(dom(5), dom(6), 10);
vals = fevalt(f, x, y, z);

if ( norm(vals(:), inf) > 0 )
   out = 0;
   return
end

% Slower check: The core may be nonzero, but the columns, rows, or tubes
% may be zero:
[rk1, rk2, rk3] = rank(f);
out_cols = zeros(rk1, 1);
out_rows = zeros(rk2, 1);
out_tubes = zeros(rk3, 1);
for j = 1:rk1
    out_cols(j) = iszero(fCols(:, j));
end
for j = 1:rk2
    out_rows(j) = iszero(fRows(:, j));
end
for j = 1:rk3
    out_tubes(j) = iszero(fTubes(:, j));
end
out = ( all(out_cols) || all(out_rows) || all(out_tubes) );

end


% op_addrcvrs.m
% Jamie Near, McGill University 2014.
%
% USAGE:
% [out,fids_presum,specs_presum,ph,sig]=op_addrcvrs(in,point,mode,coilcombos);
%
% DESCRIPTION:
% Perform weighted coil recombination for MRS data acquired with a reciever
% coil array.
%
% INPUTS:
% in            = input spectrum in matlab structure format.
% point         = point of fid to use for phase estimation (optional. Default = 1);
% mode          = Method for estimating the coil weights and phases (optional.  Default = 'w').
%                 -'w' performs amplitude weighting of channels based on the
%                 maximum signal of each coil channel.
%                 -'h' performs amplitude weighting of channels based on the
%                 maximum signal of each coil channel divided by the square of
%                 the noise in each coil channel (as described by Hall et al.
%                 Neuroimage 2014).
% coilcombos    = The predetermined coil weights and phases and amplitudes as
%                 generated by the op_getcoilcombos.m function.  If this
%                 argument is provided, the 'point', and 'mode', arguments
%                 will be ignored.  (optional.  Default = []).
%
% OUTPUTS:
% out           = Output dataset with coil channels combined.
% fids_presum   = Input data with coil channels in phase (time domain).
% specs_presum  = Input data with coil channels in phase (frequency domain).
% ph            = Vector of applied coil phases (in degrees).
% sig           = Vector of coil weights.

function [out,fids_presum,specs_presum,ph,sig]=op_addrcvrs(in,point,mode,coilcombos);

if in.flags.addedrcvrs || ~in.dims.coils
    disp('WARNING:  Only one receiver channel found!  Returning input without modification!');
    out=in;
    out.flags.addedrcvrs=1;
    fids_presum=in.fids;
    specs_presum=in.specs;
    ph=0;
    sig=1;
else

    %To get best possible SNR, add the averages together (if it hasn't already been done):
    if ~in.flags.averaged
        av=op_averaging(in);
    else
        av=in;
    end

    %also, for best results, we will combine all subspectra:
    if nargin<4
        if in.flags.isFourSteps
            av=op_fourStepCombine(av);
        end
        if in.dims.subSpecs>0
            av=op_combinesubspecs(av,'summ');
        end
        if nargin<3
            mode = 'w';
            if nargin<2
                point=1;
            end
        end
    end
    avfids=av.fids;
    avspecs=av.specs;

    %initialize phase matrix and the amplitude maxtrix that are the size of nPoints x Coils
    %ph=ones(in.sz(in.dims.t),in.sz(in.dims.coils));
    %sig=ones(in.sz(in.dims.t),in.sz(in.dims.coils));

    %now start finding the relative phases between the channels and populate
    %the ph matrix
    for n=1:in.sz(in.dims.coils)
        if nargin<4
            %ph(:,n)=phase(avfids(point,n,1,1))*ph(:,n);
            phs(n)=phase(avfids(point,n,1,1));
            switch mode
                case 'w'
                    %sig(:,n)=abs(avfids(point,n,1,1))*sig(:,n);
                    sigs(n)=abs(avfids(point,n,1,1));
                case 'h'
                    S=max(abs(avfids(:,n,1,1)));
                    N=std(avfids(end-100:end,n,1,1));
                    %sig(:,n)=(S/(N.^2))*sig(:,n);
                    sigs(n)=(S/(N.^2));
            end
        else
            %ph(:,n)=coilcombos.ph(n)*ph(:,n);
            phs(n)=coilcombos.ph(n);
            sigs(n)=coilcombos.sig(n);
        end
    end

    %now replicate the phase matrix to equal the size of the original matrix:
    % replicate=in.sz;
    % replicate(1)=1;
    % replicate(in.dims.coils)=1;
    % ph=repmat(ph,replicate);
    % sig=repmat(sig,replicate);
    sigs=sigs/norm(sigs(:));

    ph=ones(in.sz);
    sig=ones(in.sz);

    if in.dims.coils==1
        for n=1:in.sz(1)
            ph(n,:)=phs(n)*ph(n,:);
            sig(n,:)=sigs(n)*sig(n,:);
        end
    elseif in.dims.coils==2
        for n=1:in.sz(2)
            ph(:,n,:)=phs(n)*ph(:,n,:);
            sig(:,n,:)=sigs(n)*sig(:,n,:);
        end
    elseif in.dims.coils==3
        for n=1:in.sz(3)
            ph(:,:,n,:)=phs(n)*ph(:,:,n,:);
            sig(:,:,n,:)=sigs(n)*sig(:,:,n,:);
        end
    elseif in.dims.coils==4
        for n=1:in.sz(4)
            ph(:,:,:,n,:)=phs(n)*ph(:,:,:,n,:);
            sig(:,:,:,n,:)=sigs(n)*sig(:,:,:,n,:);
        end
    elseif in.dims.coils==5
        for n=1:in.sz(5)
            ph(:,:,:,:,n)=phs(n)*ph(:,:,:,:,n);
            sig(:,:,:,:,n)=sigs(n)*sig(:,:,:,:,n);
        end
    end

    %now apply the phases by multiplying the data by exp(-i*ph);
    fids=in.fids.*exp(-i*ph);
    fids_presum=fids;
    specs_presum=fftshift(ifft(fids,[],in.dims.t),in.dims.t);

    %Apply the amplitude factors by multiplying the data by amp;
    if mode=='w' || mode=='h'
        fids=fids.*sig;
    end

    %now sum along coils dimension
    fids=sum(fids,in.dims.coils);
    fids=squeeze(fids);

    %re-calculate Specs using fft
    specs=fftshift(ifft(fids,[],in.dims.t),in.dims.t);

    %change the dims variables
    if in.dims.t>in.dims.coils
        dims.t=in.dims.t-1;
    else
        dims.t=in.dims.t;
    end
    dims.coils=0;
    if in.dims.averages>in.dims.coils
        dims.averages=in.dims.averages-1;
    else
        dims.averages=in.dims.averages;
    end
    if in.dims.subSpecs>in.dims.coils
        dims.subSpecs=in.dims.subSpecs-1;
    else
        dims.subSpecs=in.dims.subSpecs;
    end
    if in.dims.extras>in.dims.coils
        dims.extras=in.dims.extras-1;
    else
        dims.extras=in.dims.extras;
    end

    %re-calculate the sz variable
    sz=size(fids);

    %FILLING IN DATA STRUCTURE
    out=in;
    out.fids=fids;
    out.specs=specs;
    out.sz=sz;
    out.dims=dims;

    %FILLING IN THE FLAGS
    out.flags=in.flags;
    out.flags.writtentostruct=1;
    out.flags.addedrcvrs=1;

end

end


%==============================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% function [vc,yc,wc,MLhis] = MLLDDMM(ML,varargin)
%
% Multi-Level for Large Deformation Diffeomorphic Metric Mapping (LDDMM)
%
% minimizes J^h(y) = D^h(T(y),R) + S^h(y) for h=coarse:fine
%
% Input:
%   ML         coarse to fine representations of the data, see getMultilevel.m
%   varagin    optional parameters, see default parameter
% Output:
%   vc          numerical optimizer (velocity field)
%   yc          optimal transformation
%   wc          optimal parameters for PIR
%   MLhis       iteration history
%   para        additional info from objective function
%
% for level=minLevel:maxLevel,
%   get data(level)
%   if level==minLevel, get wOpt running PIR; end;
%   use pre-registered Yref=trafo(wOpt,X) for regularization
%   if level==minLevel
%     v0 = trafo('get','w0'); % use this as starting guess
%   else
%     get v0 by prologating vopt from coarse to finer level
%   end;
%   get vopt using GaussNewton using v0 as starting guess
% end%For
%
%==============================================================================
%
% optimizer are controlled via [default]
%    PIRobj      [= @PIRobjFctn;         ]
%    PIRpara     [= optPara('PIR-GN');   ]
%    NPIRobj     [= @NPIRobjFctn;         ]
%    NPIRpara    [= optPara('NPIR-GN')   ]
%
% see also Ex_MLLDDMM.m for an example
%==============================================================================

function [vc,yc,wc,MLhis] = MLLDDMM(ML,varargin)

if nargin == 0
  % run minimal example
  help(mfilename);
  Ex_MLLDDMM;
  yc = 'endOfMinimalExample';
  return;
end;

%------------------------------------------------------------------------------
% initialize some default parameter:
% 1.1  initialize output
% 1.2  determine minLevel and maxLevel from the data ML
% 1.3  flag for parametric registration (PIR)
% 2.1  regularization of of PIR objective
% 2.2  regularization of Hessian approximation, H -> H + beta*speye(size(H))
% 2.3  setup stopping value wStop and initial value w0
% 2.4  setup objective in PIR  and optimization parameters
% 3    setup objective in NPIR and optimization parameters
% 4       setup defaults for plots
% 5    replace defaults by user input
% 6    initializes dimension, discretization, grids etc
% 7    convert optimization parameter sets for PIR and NPIR to lists
%------------------------------------------------------------------------------
% 1.1 initialize output
vc          = [];
yc          = [];
wc          = [];
MLhis       = [];

% 1.2  get minLevel and maxLevel from ML
[ML,minLevel,maxLevel] = getMultilevel(ML);

% 1.3  flag for parametric registration (PIR)
parametric  = 1;     % flag: if true, run parametric pre-registration

% 2.1  regularization of PIR objective
%   J_{PIR} = D(T(Y(wc)),Rc) + S(wc) with S_{PIR} = 0.5*(wc-wRef)'*M*(wc-wRef);
wRef        = [];
M           = [];

% 2.2  regularization of Hessian approximation, H -> H + beta*speye(size(H))
beta        = 0;

% 2.3  setup stopping value wStop and initial value w0
wStop       = [];           % global stopping for PIR
w0          = [];           % starting guess for PIR

% 2.4  PIR: default objective function and optimization parameter via optPara('PIR-GN')
PIRobj      = @PIRobjFctn;
PIRpara     = optPara('PIR-GN');

% 3.   NPIR: default objective function and optimization parameter via optPara('NPIR-GN')
NPIRobj     = @LDDMMobjFctn;
NPIRpara    = optPara('NPIR-GN');

% 4       setup defaults for plots
pause       = 0;        % flag for pauses
plots       = 1;        % flag for plots
plotIter    = 0;        % flag for output of iteration history each level
plotMLiter  = 0;        % flag for output of summarized iteration history

omegaV      = [];       % domain for velocity field
N           = 5;        % number of forward euler steps
mV          = @(m) m;   % resolution of velocity field
nt          = regularizer('get','nt');        % number of time discretizations for velocities
vStop       = [];           % global stopping for NPIR
vRef        = [];           % regularization: S(vc-vRef)

% 5    replace defaults by user input
for k=1:2:length(varargin) % overwrites default parameter
  eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

% 6    initializes dimension, discretization, grids etc
dimstr      = @(m) sprintf('[%s]',sprintf(' %d',m));
omega       = ML{end}.omega;  % spatial domain
grid        = regularizer('get','grid');
switch grid
  case 'cell-centered', getGrid = @(m) getCellCenteredGrid(omega,m);
  case 'staggered',     getGrid = @(m) getStaggeredGrid(omega,m);
  case 'nodal',         getGrid = @(m) getNodalGrid(omega,m);
  otherwise, error('nyi');
end;

% 7    convert optimization parameter sets for PIR and NPIR to lists
PO = FAIRcell2struct(PIRpara);
NO = FAIRcell2struct(NPIRpara);
%------------------------------------------------------------------------------

fprintf('%s: MultiLevel Large Deformation Diffeomorphic Metric Mapping\n',mfilename)
fprintf('>> %-20s : %s\n','omega',dimstr(omega));
fprintf('>> %-20s : %s\n','IMAGE MODEL',imgModel);
fprintf('>> %-20s : %s\n','DISTANCE',distance);
fprintf('>> %-20s : %s\n','TRAFO',trafo);
fprintf('>> %-20s : %s, alpha=%s\n','REGULARIZER',...
  regularizer,num2str(regularizer('get','alpha')));
fprintf('>> %-20s : %d\n','PRE-REGISTRATION',parametric);
fprintf('>> %-20s : objective=<%s>, method = <%s>\n',...
  'PIR',func2str(PIRobj),func2str(PIRpara.scheme));
fprintf('>> %-20s : objective=<%s>, method = <%s>\n',...
  'NPIR',func2str(NPIRobj),func2str(NPIRpara.scheme));
fprintf('>> %-20s : minLevel=%d, maxLevel=%d\n','LEVEL',minLevel,maxLevel);
FAIRmessage('#')

tic;
%--------------------------------------------------------------------------
for level=minLevel:maxLevel

  FAIRmessage(sprintf('%s: level %d from %d to %d, %s',...
    mfilename,level,minLevel,maxLevel,dimstr(ML{level}.m)));

  % save(old grid), update(m,grid,data coefficients)
  m     = ML{level}.m;
  xc    = getGrid(m);
  [T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega);
  Rc    = imgModel(R,omega,center(xc,m));
  if strcmp(func2str(NPIRobj),'MPLDDMMobjFctn')
      T = imgModel(T,omega,center(xc,m));
  end

  if level == minLevel    % on coarse level

    if parametric          % perform parametric pre-registration

      % initialize plots for PIR
      FAIRplots('set','mode','PIR','fig',minLevel-1,'plots',plots);
      FAIRplots('init',struct('Tc',T,'Rc',R,'omega',omega,'m',m));

      % ----- call Parametric Image Registration ----------------------------
      xC = getGrid(m);
      PIRfctn = @(wc) PIRobj(T,Rc,omega,m,beta,M,wRef,xC,wc);
      PIRfctn([]); % report status of objective function
      if isempty(w0),    w0    = trafo('w0'); end;
      if isempty(wStop), wStop = w0;          end;

      [wc,hisPIR] = PIRpara.scheme(PIRfctn,w0,'yStop',wStop,PO{:});
      fprintf('wc = \n');
      disp(wc');
      doPause(pause);
      % ----- end PIR --------------------------------------

      if plotIter
        hisPIR.str{1} = 'iteration history for PIR';
        plotIterationHistory(hisPIR,'J',1:4,'fh',100+minLevel-1);
      end;
    else                    % no pre-registration
      wc = [];
    end;

  end;

  % compute xc = trafo(wc,xc)
  if ~isempty(wc) % use yRef(x) = x
    xc = trafo(wc,xc);
  end

  % compute starting guess y0 for this level and stopping value yStop
  if level == minLevel
      if isempty(vRef)
          vRef = getVelocityStartingGuess(omegaV,mV(m),nt);
      end
      v0 = vRef;  % the best known so far
  else
    % prolongate vc (coarse) vRef (current)
    vcOld   = reshape(vc,[],nt+1);
    vRefOld = reshape(vRef,[],nt+1);
    v0 = []; vRef = [];
    for k=1:nt+1
        v0   = [v0 mfPu(vcOld(:,k),omega,mV(m/2))];
        vRef = [vRef mfPu(vRefOld(:,k),omega,mV(m/2))];
    end
    v0 = v0(:); vRef = vRef(:);
 end;
    vStop = 0*getVelocityStartingGuess(omegaV,mV(m),nt);

  % ----- call NPIR -------------------------------------
  % initialize plots for NPIR
  FAIRplots('reset','mode','NPIR','fig',level,'plots',plots);
  FAIRplots('init',struct('Tc',T,'Rc',R,'omega',omega,'m',m));

  NPIRfctn = @(vc) NPIRobj(T,Rc,omega,m,vRef,xc,omegaV,mV(m),N,vc);
  if level == minLevel   % report status of objective function
    NPIRfctn([]);
  end; %

  [vc,his] = NPIRpara.scheme(NPIRfctn,v0,'yStop',vStop,NO{:});
  % ----- end NPIR --------------------------------------

  if plotIter
    his.str{1} = sprintf('iteration history for LDDMM, level=%d',level);
    plotIterationHistory(his,'J',1:4,'fh',100+level);
  end;

  % update iteration history
  if level == minLevel
    MLhis.str = his.str;
    MLhis.his = his.his;
  else
    MLhis.his = [MLhis.his;his.his];
  end;
  doPause(pause);

%--------------------------------------------------------------------------
end;%For level
%--------------------------------------------------------------------------

MLhis.time = toc;

if plotMLiter
  plotMLIterationHistory(MLhis,'fh',[]);
end;

id0 = find(MLhis.his(:,1)==-1,1,'last');
MLhis.reduction = MLhis.his(end,2)/MLhis.his(id0,2);
J = find(MLhis.his(:,1)==-1);
MLhis.iter(minLevel:maxLevel) = MLhis.his([J(2:end)-1;size(MLhis.his,1)],1)';

FAIRmessage([mfilename,' : done !'],'#');

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

function doPause(p)
if strcmp(p,'on')
  FAIRpause;
elseif p>0
  FAIRpause(p);
end;

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


function [gci,goi] = v_dypsa(s,fs)
%V_DYPSA   Derive glottal closure instances from speech [gci,goi] = (s,fs)
%   Note: Needs to be combined with a voiced-voiceless detector to eliminate
%   spurious closures in unvoiced and silent regions.
%
%   Inputs:
%   s     is the speech signal
%   fs    is the sampling frequncy
%
%   Outputs:
%   gci   is a vector of glottal closure sample numbers
%   gco   is a vector of glottal opening sample numbers derived from
%         an assumed constant closed-phase fraction
%
%   References:
%   [1]  P. A. Naylor, A. Kounoudes, J. Gudnason, and M. Brookes, "Estimation of Glottal Closure
%        Instants in Voiced Speech using the DYPSA Algorithm," IEEE Trans on Speech and Audio
%        Processing, vol. 15, pp. 34-43, Jan. 2007.
%   [2]  M. Brookes, P. A. Naylor, and J. Gudnason, "A Quantitative Assessment of Group Delay Methods
%        for Identifying Glottal Closures in Voiced Speech," IEEE Trans on Speech & Audio Processing,
%        vol. 14, no. 2, pp. 456-466, Mar. 2006.
%   [3]  A. Kounoudes, P. A. Naylor, and M. Brookes, "The DYPSA algorithm for estimation of glottal
%        closure instants in voiced speech," in Proc ICASSP 2002, vol. 1, Orlando, 2002, pp. 349-352.
%   [4]  C. Ma, Y. Kamp, and L. F. Willems, "A Frobenius norm approach to glottal closure detection
%        from the speech signal," IEEE Trans. Speech Audio Processing, vol. 2, pp. 258-265, Apr. 1994.
%   [5]  A. Kounoudes, "Epoch Estimation for Closed-Phase Analysis of Speech," PhD Thesis,
%        Imperial College, 2001.

% Algorithm Parameters
%       The following parameters are defined in v_voicebox()
%
%   dy_cpfrac=0.3;           % presumed closed phase fraction of larynx cycle
%   dy_cproj=0.2;            % cost of projected candidate
%   dy_cspurt=-0.45;         % cost of a talkspurt
%   dy_dopsp=1;              % Use phase slope projection (1) or not (0)?
%   dy_ewdly=0.0008;         % window delay for energy cost function term [~ energy peak delay from closure] (sec)
%   dy_ewlen=0.003;          % window length for energy cost function term (sec)
%   dy_ewtaper=0.001;        % taper length for energy cost function window (sec)
%   dy_fwlen=0.00045;        % window length used to smooth group delay (sec)
%   dy_fxmax=500;            % max larynx frequency (Hz)
%   dy_fxmin=50;             % min larynx frequency (Hz)
%   dy_fxminf=60;            % min larynx frequency (Hz) [used for Frobenius norm only]
%   dy_gwlen=0.0030;         % group delay evaluation window length (sec)
%   dy_lpcdur=0.020;         % lpc analysis frame length (sec)
%   dy_lpcn=2;               % lpc additional poles
%   dy_lpcnf=0.001;          % lpc poles per Hz (1/Hz)
%   dy_lpcstep=0.010;        % lpc analysis step (sec)
%   dy_nbest=5;              % Number of NBest paths to keep
%   dy_preemph=50;           % pre-emphasis filter frequency (Hz) (to avoid preemphasis, make this very large)
%   dy_spitch=0.2;           % scale factor for pitch deviation cost
%   dy_wener=0.3;            % DP energy weighting
%   dy_wpitch=0.5;           % DP pitch weighting
%   dy_wslope=0.1;           % DP group delay slope weighting
%   dy_wxcorr=0.8;           % DP cross correlation weighting
%   dy_xwlen=0.01;           % cross-correlation length for waveform similarity (sec)

%   Revision History:
%
%   3.0 - 29 Jun 2006  - Rewrote DP function to improve speed
%   2.6 - 29 Jun 2006  - Tidied up algorithm parameters
%   2.4 - 10 Jun 2006  - Made into a single file aand put into VOICEBOX
%   2.3 - 18 Mar 2005  - Removed 4kHz filtering of phase-slope function
%   2.2 - 05 Oct 2004  -  dpgci uses the slopes returned from xewgrdel
%                      -  gdwav from speech with fs<9000 is not filtered
%                      -  Various outputs and inputs of functions have been
%                         removed since now there is no plotting
%   1.0 - 30 Jan 2001  - Initial version [5]

%   Bugs:
%         1. Allow the projections only to extend to the end of the larynx cycle
%         2. Compensate for false pitch period cost at the start of a voicespurt
%         3. Should include energy and phase-slope costs for the first closure of a voicespurt
%         4. should delete candidates that are too close to the beginning or end of speech for the cost measures
%            currently this is 200 samples fixed in the main routine but it should adapt to window lengths of
%            cross-correlation, lpc and energy measures.
%         5. Should have an integrated voiced/voiceless detector
%         6. Allow v_dypsa to be called in chunks for a long speech file
%         7. Do forward & backward passes to allow for gradient descent and/or discriminative training
%         8. Glottal opening approximation does not really belong in this function
%         9. The cross correlation window is asymmetric (and overcomplex) for no very good reason
%        10. Incorporate -0.5 factor into dy_wxcorr and abolish erroneous (nx2-1)/(nx2-2) factor
%        11. Add in talkspurt cost at the beginning rather than the end of a spurt (more efficient)
%        12. Remove qmin>2 condition from voicespurt start detection (DYPSA 2 compatibility) in two places
%        13. Include energy and phase-slope costs at the start of a voicespurt
%        14. Single-closure voicespurt are only allowed if nbest=1 (should always be forbidden)
%        15. Penultimate closure candidate is always acceptd
%        16. Final element of gcic, Cfn and Ch is unused
%        17. Needs to cope better with irregular voicing (e.g. creaky voice)
%        18. Should give non-integer GCI positions for improved accuracy
%        19. Remove constraint that first voicespurt cannot begin until qrmax after the first candidate

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

% Extract algorithm constants from VOICEBOX

dy_preemph=v_voicebox('dy_preemph');
dy_lpcstep=v_voicebox('dy_lpcstep');
dy_lpcdur=v_voicebox('dy_lpcdur');
dy_dopsp=v_voicebox('dy_dopsp');              % Use phase slope projection (1) or not (0)?
dy_ewtaper=v_voicebox('dy_ewtaper');        % Prediction order of FrobNorm method  in seconds
dy_ewlen=v_voicebox('dy_ewlen');        % windowlength of FrobNorm method  in seconds
dy_ewdly=v_voicebox('dy_ewdly');        % shift for assymetric speech shape at start of voiced cycle
dy_cpfrac=v_voicebox('dy_cpfrac');        % presumed ratio of larynx cycle that is closed
dy_lpcnf=v_voicebox('dy_lpcnf');          % lpc poles per Hz (1/Hz)
dy_lpcn=v_voicebox('dy_lpcn');            % lpc additional poles
dy_xwlen=v_voicebox('dy_xwlen');            % cross-correlation length for waveform similarity (sec)
dy_fxminf=v_voicebox('dy_fxminf');            % minimum pitch for Frobenius norm calculation

lpcord=ceil(fs*dy_lpcnf+dy_lpcn);       % lpc poles

%PreEmphasise input speech
s_used=filter([1 -exp(-2*pi*dy_preemph/fs)],1,s);

% perform LPC analysis, AC method with Hamming windowing
[ar, e, k] = v_lpcauto(s_used,lpcord,floor([dy_lpcstep dy_lpcdur]*fs));

if any(any(isinf(ar)))    % if the data is bad and gives infinite prediction coefficients we return with a warning
    warning('No GCIs returned');
    gci=[];
    return;
end;

% compute the prediction residual
r = v_lpcifilt(s_used,ar,k);

% compute the group delay function:  EW method from reference [2] above
[zcr_cand,sew,gdwav,toff]=xewgrdel(r,fs);
gdwav=-[zeros(toff,1); gdwav(1:end-toff)];
zcr_cand=[round(zcr_cand), ones(size(zcr_cand))];   %flag zero crossing candidates with ones

sew=0.5+sew';  %the phase slope cost of each candidate

pro_cand=[];
if dy_dopsp ~= 0
    pro_cand = psp(gdwav,fs);
    pro_cand = [pro_cand, zeros(length(pro_cand),1)]; %flag projected candidates with zeros
    sew =      [sew zeros(1,size(pro_cand,1))];      %the phase slope cost of a projected candidate is zero
end;

%Sort the zero crossing and projected candidates together and remove any candidates that
%are to close to the start or end of the speech signal because the cost functions
%need room either side.

[gcic,sin] = sortrows([zcr_cand; pro_cand],1);
sew=sew(sin);
saf=max([200,dy_xwlen*fs/2+1,fs/dy_fxminf]);
sin=find(and(saf<gcic,gcic<length(gdwav)-saf));
gcic=gcic(sin,:);
sew=sew(sin);

% compute the frobenious norm function used for a cost in the DP
fnwav=frobfun(s_used,dy_ewtaper*fs,dy_ewlen*fs,dy_ewdly*fs);

%Dynamic programming, picks the most likely candidates based on the pitch consistency, energy etc.
[gci] = dpgci(gcic, s_used(:), sew, fnwav, fs);

%Evaluate goi ... determined to be dy_cpfrac percentage of the larynx cylce away from the last gci
goi=simplegci2goi(gci,dy_cpfrac);

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

function z = psp(g,fs)
%PSP  Calculates the phase slope projections of the group delay function
%   Z = PSP(G) computes the

%   Revision History:
%       V1.0 July 12th 2002:
%            Nov. 6th 2002 : if statement added to remove "last" midpoint

g = g(:);

gdot = [diff(g);0];
gdotdot = [diff(gdot);0];

% find the turning points  as follows: [tp_number, index_of_tp, min(1) or max(-1), g(index_of_tp)]
turningPoints = zcr(gdot);
turningPoints = [[1:length(turningPoints)]', turningPoints, sign(gdotdot(turningPoints)), g(turningPoints)];

% useful for debug/plotting
%tplot = zeros(length(g),1);
%tplot(turningPoints(:,1)) = turningPoints(:,2);

% find any maxima which are < 0
negmaxima = turningPoints(find(turningPoints(:,3) == -1 & turningPoints(:,4) < 0 & turningPoints(:,1)~=1),:);  %Change 01.05.2003 JG: The first row can't be included

% find the midpoint between the preceding min and the negative max
nmi = negmaxima(:,1);
midPointIndex = turningPoints(nmi-1,2) + round(0.5*(turningPoints(nmi,2) - turningPoints(nmi-1,2)));
midPointValue = g(midPointIndex);

% project a zero crossing with unit slope
nz = midPointIndex - round(midPointValue);

% find any minima which are > 0
posminima = turningPoints(find(turningPoints(:,3) == 1 & turningPoints(:,4) > 0),:);

% find the midpoint between the positive min and the following max
pmi = posminima(:,1);

%Remove last midpoint if it is the last sample
if ~isempty(pmi), if pmi(end)==size(turningPoints,1), pmi=pmi(1:end-1); end; end;

midPointIndex = turningPoints(pmi,2) + round(0.5*(turningPoints(pmi+1,2) - turningPoints(pmi,2)));
midPointValue = g(midPointIndex);

% project a zero crossing with unit slope
pz = midPointIndex - round(midPointValue);

z = sort([nz;pz]);

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

function i = zcr(x, p)
%ZCR  Finds the indices in a vector to  zero crossings
%   I = ZCR(X) finds the indices of vector X which are closest to zero-crossings.
%   I = ZCR(X, P) finds indices for positive-going zeros-crossings for P=1 and
%   negative-going zero-crossings for P=0.

x = x(:);

if (nargin==2)
    if (p==0)
        z1 = zcrp(x);   % find positive going zero-crossings
    elseif (p==1)
        z1 = zcrp(-x);  % find negative going zero-crossings
    else
        error('ZCR: invalid input parameter 2: must be 0 or 1');
    end
else
    z1 = [zcrp(x); zcrp(-x)];
end

% find crossings when x==0 exactly
z0 = find( (x(1:length(x))==0) & ([x(2:length(x));0] ~= 0));

% concatenate and sort the two types of zero-crossings
i = sort([z0; z1]);

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

function zz = zcrp(xx)  %only used in zcr
% find positive-going zero-crossing
z1 = find(diff(sign(xx)) == -2);
% find which out of current sample or next sample is closer to zero
[m, z2] = min([abs(xx(z1)), abs(xx(z1+1))], [], 2);
zz =  z1 -1 + z2;

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

function [frob]=frobfun(sp,p,m,offset)

% [frob]=frobfun(sp,p,m)
%
% sp is the speech signal assumed to be preemphasised
% p  is the prediction order  : recomended to be 1 ms in above paper
% m  is the window length     : recomended to be 1 ms in above paper
% offset is shift for assymetric speech shape at start of voiced cycle -
% default 1.5ms.
%
% This function implements the frobenius norm based measure C defined in [4] below.
% It equals the square of the Frobenius norm of the m by p+1 data matrix divided by p+1
%
% Reference:
%   [4]  C. Ma, Y. Kamp, and L. F. Willems, "A Frobenius norm approach to glottal closure detection
%        from the speech signal," IEEE Trans. Speech Audio Processing, vol. 2, pp. 258"265, Apr. 1994.

%   Revision History:
%       V1.0 July 12th 2002:
%            Nov. 6th 2002 : if statement added to remove "last" midpoint

%force p m and offset to be integers
p=round(p);
m=round(m);
offset=round(offset);

w=(p+1)*ones(1,m+p);
w(1:p)=1:p;
w(m+1:p+m)=p:-1:1;

w=w./(p+1);
frob=filter(w,1,sp.^2);
frob(1:(round((p+m-1)/2) + offset))=[];

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

function goi=simplegci2goi(gci,pr)

% Estimate glottal opening instants by assuming a fixed closed-phase fraction

gci=round(gci);
maxpitch=max(medfilt1(diff(gci),7));

% calculate opening instants
for kg=1:length(gci)-1
    goi(kg)=gci(kg)+min(pr*(gci(kg+1)-gci(kg)),pr*maxpitch);
end;
kg=kg+1;
goi(kg)=round(gci(kg)+pr*(gci(kg)-gci(kg-1)));  %use the previous pitch period instead
goi=round(goi);

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

function [tew,sew,y,toff]=xewgrdel(u,fs)

% implement EW group delay epoch extraction

dy_gwlen=v_voicebox('dy_gwlen');          % group delay evaluation window length
dy_fwlen=v_voicebox('dy_fwlen');          % window length used to smooth group delay

% perform group delay calculation

gw=2*floor(dy_gwlen*fs/2)+1;            % force window length to be odd
ghw=window('hamming',gw,'s');
ghw = ghw(:);                           % force to be a column (dmb thinks window gives a row - and he should know as he wrote it!)
ghwn=ghw'.*(gw-1:-2:1-gw)/2;            % weighted window: zero in middle

u2=u.^2;
yn=filter(ghwn,1,u2);
yd=filter(ghw,1,u2);
yd(abs(yd)<eps)=10*eps;                 % prevent infinities
y=yn(gw:end)./yd(gw:end);               % delete filter startup transient
toff=(gw-1)/2;
fw=2*floor(dy_fwlen*fs/2)+1;            % force window length to be odd
if fw>1
    daw=window('hamming',fw,'s');
    y=filter(daw,1,y)/sum(daw);         % low pass filter
    toff=toff-(fw-1)/2;
end
[tew,sew]=v_zerocros(y,'n');              % find zero crossings

tew=tew+toff;                           % compensate for filter delay and frame advance

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

function Cfn=fnrg(gcic,frob,fs)

%Frobenious Energy Cost

dy_fxminf=v_voicebox('dy_fxminf');
frob=frob(:)';
mm=round(fs/dy_fxminf);
mfrob=v_maxfilt(frob,1,mm);
mfrob=[mfrob(floor(mm/2)+1:end) max(frob(end-ceil(mm/2):end))*ones(1,floor(mm/2))];
rfr=frob./mfrob;
Cfn=0.5-rfr(round(gcic));

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

function gci=dpgci(gcic, s, Ch, fnwav, fs)

%DPGCI   Choose the best Glottal Closure Instances with Dynamic Programming
%   gci=dpgci(gcic, s(:), Ch, fnwav, fs) returns vectors of sample indices corresponding
%   to the instants of glottal closure in the speech signal s at sampling frequency fs Hz.
%
%   Inputs:
%   gcic    is a matrix whos first column are the glottal closure instance candidates and
%           the second column is 1 if the corresponding gci is derived from a zero crossing
%           but zero if the gci is from a a projected zero crossing
%   s       is the speech signal - MUST be a column vector
%   Ch      the phase slope cost of every candidate
%   fnwav   is the frobenious norm function of s
%   fs      is the sampling frequncy
%
%   Outputs:
%   gci     is a vector of glottal closure instances chosen by the DP

%   Revision History:
%   Bugs:  Constants are hardwired but defined in a structure like pv (defined in grpdelpv)
%

% get algorithm parameters from v_voicebox()

dy_fxmin=v_voicebox('dy_fxmin');        % min larynx frequency (Hz)
dy_fxmax=v_voicebox('dy_fxmax');        % min larynx frequency (Hz)
dy_xwlen=v_voicebox('dy_xwlen');        % cross-correlation length for waveform similarity (sec)
dy_nbest=v_voicebox('dy_nbest');        % Number of NBest paths to keep
dy_spitch=v_voicebox('dy_spitch');              % scale factor for pitch deviation cost
wproj=v_voicebox('dy_cproj');           % cost of projected candidate
dy_cspurt=v_voicebox('dy_cspurt');           % cost of a talkspurt
dy_wpitch=v_voicebox('dy_wpitch');           % DP pitch weighting
dy_wener=v_voicebox('dy_wener');           % DP energy weighting
dy_wslope=v_voicebox('dy_wslope');           % DP group delay slope weighting
dy_wxcorr=v_voicebox('dy_wxcorr');           % DP cross correlation weighting

%Constants
Ncand=length(gcic);
sv2i=-(2*dy_spitch^2)^(-1);                 % scale factor for pitch deviation cost
nxc=ceil(dy_xwlen*fs);                      % cross correlation window length in samples
% === should delete any gci's that are within this of the end.
% === and for energy window

%Limit the search:
qrmin=ceil(fs/dy_fxmax);
qrmax=floor(fs/dy_fxmin);

%Cost and tracking r = current, q = previous, p = preprevious
cost=zeros(Ncand, dy_nbest); cost(:,:)=inf;    %Cost matrix, one row for each candidate
maxcost=zeros(Ncand,1); maxcost(:,:)=inf;   %Maximum cost in each row
imaxcost=ones(Ncand,1);                     %Index of maximum cost

prev = ones(Ncand, dy_nbest);                  %index of previous, q candidates
ind = ones(Ncand, dy_nbest);                   %index of p in row q (from prev)
qbest = [zeros(Ncand,1), ones(Ncand,2)]; % the minimum cost in any previous q [cost,q,i]

Cfn=fnrg(gcic(:,1),fnwav,fs);  %Frob.Energy Cost

%Add start and end state
% === should probably delete candidates that are too close to either end of the input
% === why do we ever need the additional one at the tail end ?
gcic=[[gcic(1,1)-qrmax-2 0];gcic;[gcic(end,1)+qrmax+2 0]];
Cfn=[0 Cfn 0];
Ch = [0 Ch 0];

% first do parallelized version

% rather complicated window specification is for compatibility with DYPSA 2
% === +1 below is for compatibility - probably a bug
wavix=(-floor(nxc/2):floor(nxc/2)+1)';                 % indexes for segments [nx2,1]
nx2=length(wavix);
sqnx2=sqrt(nx2);
g_cr=dy_wener*Cfn+dy_wslope*Ch+wproj*(1-gcic(:,2))';  % fixed costs

g_n=gcic(:,1)';                  % gci sample number [1,Ncand+2]
g_pr=gcic(:,2)';                 % unprojected flag [1,Ncand+2]
g_sqm=zeros(1,Ncand+1);         % stores: sqrt(nx2) * mean for speech similarity waveform
g_sd=zeros(1,Ncand+1);         % stores: 1/(Std deviation * sqrt(nx2)) for speech similarity waveform
f_pq=zeros((Ncand+1)*dy_nbest,1);   % (q-p) period for each node
f_c=repmat(Inf,(Ncand+1)*dy_nbest,1);    % cumulative cost for each node - initialise to inf
f_c(1)=0;                       % initial cost of zero for starting node
% f_costs=zeros(Ncand*dy_nbest,6);   % === debugging only remember costs of candidate
f_f=ones((Ncand+1)*dy_nbest,1);    % previous node in path
f_fb=ones((Ncand+1),1);    % points back to best end-of-spurt node
fbestc=0;                       % cost of best end-of-spurt node

qmin=2;
for r=2:Ncand+1
%     if r==86
%         r;
%     end
    r_n=g_n(r);             % sample number of r = current candidate
    rix=dy_nbest*(r-1)+(1:dy_nbest);    % index range within node variables

    % determine the range of feasible q candidates
    qmin0=qmin;
    qmin=find(g_n(qmin0-1:r-1)<r_n-qrmax);      % qmin is the nearest candidate that is >qrmax away
    qmin=qmin(end)+qmin0-1;             % convert to absolute index of first viable candidate
    qmax=find(g_n(qmin-1:r-1)<=r_n-qrmin);      % qmax is the nearest candidate that is >=qrmin away
    qmax=qmax(end)+qmin-2;

    % calculate waveform similarity cost measure statistics

    sr=s(r_n+wavix);        % note s MUST be a column vector so sr is also
    wsum=sum(sr);
    g_sqm(r)=wsum/sqnx2;                % mean * sqrt(nx2)
    g_sd(r)=1/sqrt(sr.'*sr-wsum^2/nx2);   % 1/(Std deviation * sqrt(nx2))

    % now process the candidates

    if qmin<=qmax
        qix=qmin:qmax;      % q index
        nq=length(qix);
        % === should integrate the -0.5 into dy_wxcorr
        % === the factor (nx2-1)/(nx2-2) is to compensate for a bug in swsc()
        q_cas=-0.5*(nx2-1)/(nx2-2)*dy_wxcorr*(sum(s(repmat(g_n(qix),nx2,1)+repmat(wavix,1,nq)).*repmat(sr,1,nq),1)-g_sqm(qix)*g_sqm(r)).*g_sd(qix)*g_sd(r);
        % compare: i=35; Ca=swsc(g_n(qix(i)),g_n(r),s,fs); [i qix(i) r  g_n(qix(i)) g_n(r) dy_wxcorr*Ca q_cas(i)]

        % now calculate pitch deviation cost

        fix = 1+(qmin-1)*dy_nbest:qmax*dy_nbest;    % node index range
        f_qr=repmat(r_n-g_n(qix),dy_nbest,1);    % (r-p) period for each node
        f_pr=f_qr(:)+f_pq(fix);
        % === could absorb the 2 into sv2i
        f_nx=2-2*f_pr./(f_pr+abs(f_qr(:)-f_pq(fix)));
        f_cp=dy_wpitch*(0.5-exp(sv2i*f_nx.^2));
        % === fudge to match dypsa2.4 - could more efficiently be added
        % === onto the cost of a talkspurt end
        % === should be a v_voicebox parameter anyway
        f_cp(f_pq(fix)==0)=dy_cspurt*dy_wpitch;

        % now find the N-best paths

        [r_cnb,nbix]=sort(f_c(fix)+f_cp+reshape(repmat(q_cas,dy_nbest,1),nq*dy_nbest,1));
        f_c(rix)=r_cnb(1:dy_nbest)+g_cr(r);     % costs
        f_f(rix)=nbix(1:dy_nbest)+(qmin-1)*dy_nbest;       % traceback nodes
        f_pq(rix)=f_qr(nbix(1:dy_nbest));       % previous period
        % === f_costs is only for debugging
%         r;
%         f_costs(rix,1)=f_c(fix(nbix(1:dy_nbest)));
%         f_costs(rix,2)=wproj*(1-gcic(r,2));
%         f_costs(rix,3)=f_cp(nbix(1:dy_nbest));
%         f_costs(rix,4)=dy_wener*Cfn(r);
%         f_costs(rix,5)=dy_wslope*Ch(r);
%         f_costs(rix,6)=reshape(q_cas(1+floor((nbix(1:dy_nbest)-1)/dy_nbest)),dy_nbest,1);

        % check cost of using this candidate as the start of a new spurt
        % ==== the qmin>2 condition is for compatibility with v_dypsa 2 and
        % prevents any spurts starting until at least qrmax past the first
        % gci. This is probably a bug (see again below)
        iNb=rix(end);
        if (qmin>2) && (f_c(f_fb(qmin-1))+wproj*(1-gcic(r,2))<f_c(iNb))        % compare with worst of Nbest paths
            f_f(iNb)=f_fb(qmin-1);
            % === for now we exclude the energy and phase-slope costs for compatibility with dypsa2
            % === this is probably a bug
            f_c(iNb)=f_c(f_fb(qmin-1))+wproj*(1-gcic(r,2));     % replace worst of the costs with start voicespurt cost
            f_pq(iNb)=0;                    % false pq period
        end
        if f_c(rix(1))<fbestc
            f_fb(r)=rix(1);                          % points to the node with lowest end-of-spurt cost
            % === should compensate for the pitch period cost incurred at the start of the next spurt
            % === note that a node can never be a one-node voicespurt on its own unless dy_nbest=1
            % since the start voices[purt option replaced the worst Nbest cost. This is probably good but
            % is a bit inconsistent.
            fbestc=f_c(rix(1));
        else
            f_fb(r)=f_fb(r-1);
        end
    else            % no viable candidates - must be the start of a voicespurt if anything
        % === for now we exclude the energy and phase-slope costs for compatibility with dypsa2
        % === this is probably a bug
        % ==== the qmin>2 condition is for compatibility with v_dypsa 2 and
        % prevents any spurts starting until at least qrmax past the first
        % gci. This is probably a bug (see again above)
        if (qmin>2)
            f_c(rix(1))=f_c(f_fb(qmin-1))+wproj*(1-gcic(r,2));  % cost of new voicespurt
            f_f(rix)=f_fb(qmin-1);                              % traceback to previous talkspurt end
            f_pq(rix)=0;                                        % previous period
        end
        f_fb(r)=f_fb(r-1);                                  % cannot be the end of a voicespurt
    end
end

% now do the traceback

gci = zeros(1,Ncand+1);

% === for compatibility with dypsa2, we force the penultimate candidate to be accepted
% === should be: i=f_fb(Ncand+1) but instead we pick the best of the penultimate candidate
i=rix(1)-dy_nbest;
if f_c(i-dy_nbest+1)<f_c(i)     % check if start of a talkspurt
    i=i-dy_nbest+1;
end
k=1;
while i>1
    j=1+floor((i-1)/dy_nbest);          % convert node number to candidate number
    gci(k)=g_n(j);
    i=f_f(i);
    k=k+1;
end
gci=gci(k-1:-1:1);           % put into ascending order

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


% function [nll, grad] = InstanceNegLogLikelihood(X, y, theta, modelParams)
% returns the negative log-likelihood and its gradient, given a CRF with parameters theta,
% on data (X, y).
%
% Inputs:
% X            Data.                           (numCharacters x numImageFeatures matrix)
%              X(:,1) is all ones, i.e., it encodes the intercept/bias term.
% y            Data labels.                    (numCharacters x 1 vector)
% theta        CRF weights/parameters.         (numParams x 1 vector)
%              These are shared among the various singleton / pairwise features.
% modelParams  Struct with three fields:
%   .numHiddenStates     in our case, set to 26 (26 possible characters)
%   .numObservedStates   in our case, set to 2  (each pixel is either on or off)
%   .lambda              the regularization parameter lambda
%
% Outputs:
% nll          Negative log-likelihood of the data.    (scalar)
% grad         Gradient of nll with respect to theta   (numParams x 1 vector)
%
% Copyright (C) Daphne Koller, Stanford Univerity, 2012

function [nll, grad] = InstanceNegLogLikelihood(X, y, theta, modelParams)

    % featureSet is a struct with two fields:
    %    .numParams - the number of parameters in the CRF (this is not numImageFeatures
    %                 nor numFeatures, because of parameter sharing)
    %    .features  - an array comprising the features in the CRF.
    %
    % Each feature is a binary indicator variable, represented by a struct
    % with three fields:
    %    .var          - a vector containing the variables in the scope of this feature
    %    .assignment   - the assignment that this indicator variable corresponds to
    %    .paramIdx     - the index in theta that this feature corresponds to
    %
    % For example, if we have:
    %
    %   feature = struct('var', [2 3], 'assignment', [5 6], 'paramIdx', 8);
    %
    % then feature is an indicator function over X_2 and X_3, which takes on a value of 1
    % if X_2 = 5 and X_3 = 6 (which would be 'e' and 'f'), and 0 otherwise.
    % Its contribution to the log-likelihood would be theta(8) if it's 1, and 0 otherwise.
    %
    % If you're interested in the implementation details of CRFs,
    % feel free to read through GenerateAllFeatures.m and the functions it calls!
    % For the purposes of this assignment, though, you don't
    % have to understand how this code works. (It's complicated.)

    featureSet = GenerateAllFeatures(X, modelParams);

    % Use the featureSet to calculate nll and grad.
    % This is the main part of the assignment, and it is very tricky - be careful!
    % You might want to code up your own numerical gradient checker to make sure
    % your answers are correct.
    %
    % Hint: you can use CliqueTreeCalibrate to calculate logZ effectively.
    %       We have halfway-modified CliqueTreeCalibrate; complete our implementation
    %       if you want to use it to compute logZ.

    nll = 0;
    grad = zeros(size(theta));
    %%%
    % Your code here:
    n = length(y);
    K = modelParams.numHiddenStates;
    Factors = repmat(struct('var',[],'card',[],'val',[]),1,2*n-1);
    for i = 1:n-1
	    Factors(i).var = [i];
	    Factors(i).card = [K];
	    Factors(i).val = zeros(1,prod(Factors(i).card));
	    Factors(n+i).var = [i,i+1];
	    Factors(n+i).card = [K,K];
	    Factors(n+i).val = zeros(1,prod(Factors(i+n).card));
    end
    Factors(n).var = [n];
    Factors(n).card = [K];
    Factors(n).val = zeros(1,prod(Factors(n).card));

    featureCounts = zeros(size(theta));
    modelFeatureCounts = zeros(size(theta));

    for i = 1:length(featureSet.features)
	  feature = featureSet.features(i);
	  if length(feature.var) == 1
		  Factors(feature.var(1)).val(feature.assignment) += theta(feature.paramIdx);
	  else
		  % assuming feature.var is always [i,i+1].. if this fails accomodate for [i+1,i] as well
	          indx = AssignmentToIndex(feature.assignment,[K,K]);
		  fact = n + feature.var(1);
		  Factors(fact).val(indx) = theta(feature.paramIdx);
	  end

	  if feature.assignment == y([feature.var])
		  featureCounts(feature.paramIdx) = 1;
	  end
    end

    weightedFeatureCounts = featureCounts.*theta;

    logFactors = Factors;

    for i = 1:length(Factors)
	    Factors(i).val = exp(Factors(i).val);
    end

    P = CreateCliqueTree(Factors);
    [P, logZ] = CliqueTreeCalibrate(P, 0);
    lambda = modelParams.lambda;
    regularizationCost = sum(theta.*theta)*lambda/2;
    regularizationGradient = theta*lambda;

    % change the code how we do inference over all the probablilities to in case of words with letters not equal to 3.. do it using code instead of this lame crap..

    p1 = FactorMarginalization(P.cliqueList(1),2);
    p2 = FactorMarginalization(P.cliqueList(1),1);
    p3 = FactorMarginalization(P.cliqueList(2),2);
    p12 = P.cliqueList(1);
    p23 = P.cliqueList(2);
    p1.val/=sum(p1.val);
    p2.val/=sum(p2.val);
    p3.val/=sum(p3.val);
    p23.val/=sum(p23.val);
    p12.val/=sum(p12.val);
    ft = pppp(featureSet.features,p1,p2,p3,p12,p23);
    for i = 1:length(ft)
	    modelFeatureCounts(ft(i).paramIdx) += ft(i).p;
    end

    nll = logZ - sum(weightedFeatureCounts) + regularizationCost;
    grad = modelFeatureCounts - featureCounts + regularizationGradient;
end


function x = meicv2(c,N1,N2,ns,nag)
% meicv2 - 2D inverse mirror-extended curvelet transform
% -----------------
% INPUT
% --
% c is a cell array which contains the curvelets coefficients. If
% tp=='ortho', then c{j}{l}(n1,n2) is the coefficient at scale j,
% direction l and spatial index (n1,n2). The directional index l
% iterates through the wedges in the first quadrant. Notice that, for
% the mirror-extended wave atoms, the spatial indices wrap around once.
% --
% N1, N2 are positive integers.
% --
% ns is the number of levels, including the coarsest level. ns =
% ceil(log2(min(N1,N2)) - 3) is commonly used.
% --
% nag is the number of angles used for the second coarsest level.
% nag is required to be a multiple of 4 and nag = 16 is often used.
% -----------------
% OUTPUT
% --
% x is an N1-by-N2 matrix.
% -----------------
% Written by Lexing Ying and Laurent Demanet, 2007

  E1 = ceil(N1/3);  E2 = ceil(N2/3);  %E1 = 0;  E2 = 0;
  A1 = 2*(N1+E1);  A2 = 2*(N2+E2);
  fd = zeros(A1,A2);

  G1 = 4/3*N1;  G2 = 4/3*N2;

  for s=ns:-1:2
    R1 = 2^(s-ns)*G1;
    R2 = 2^(s-ns)*G2;

    idx1 = [ceil(-R1):floor(R1)];
    [wl,wr] = cvwindow((idx1+R1/1)/(R1/2));    tmpa = wl;
    [wl,wr] = cvwindow((idx1-R1/2)/(R1/2));    tmpb = wr;
    coef1 = tmpa.*tmpb;
    idx2 = [ceil(-R2):floor(R2)];
    [wl,wr] = cvwindow((idx2+R2/1)/(R2/2));    tmpa = wl;
    [wl,wr] = cvwindow((idx2-R2/2)/(R2/2));    tmpb = wr;
    coef2 = tmpa.*tmpb;
    lowpass = coef1'*coef2;

    idx1 = [ceil(-R1):floor(R1)];
    [wl,wr] = cvwindow((idx1+R1/2)/(R1/4));    tmpa = wl;
    [wl,wr] = cvwindow((idx1-R1/4)/(R1/4));    tmpb = wr;
    coef1 = tmpa.*tmpb;
    idx2 = [ceil(-R2):floor(R2)];
    [wl,wr] = cvwindow((idx2+R2/2)/(R2/4));    tmpa = wl;
    [wl,wr] = cvwindow((idx2-R2/4)/(R2/4));    tmpb = wr;
    coef2 = tmpa.*tmpb;
    tmppass = coef1'*coef2;
    hghpass = sqrt(1-tmppass.^2);

    pass = lowpass.*hghpass;
    [M1,M2] = size(pass);

    fh = zeros(M1,M2);
    %get fh

    nbangles = nag*2^(ceil((s-2)/2));
    %---------
    [M1,M2] = size(fh);
    nd = nbangles/4;
    cs = c{s};
    W1 = 2*R1/nd;  W2 = 2*R2/nd;

    %take only first quadrant
    cnt = 1;
    for g=nd/2:nd-1
      xs = R1/4-(W1/2)/4;    xe = R1;
      ys = -R2 + (2*g-1)*W2/2;		ye = -R2 + (2*g+3)*W2/2;
      xn = ceil(xe-xs);    yn = ceil(ye-ys);
      if(g==0)
        thts = atan2(-1.0, 1.0-1.0/nd);
        thtm = atan2(-1.0+1.0/nd, 1.0);
        thte = atan2(-1.0+3.0/nd, 1.0);
      elseif(g==nd-1)
        thts = atan2(-1.0+(2.0*g-1.0)/nd, 1.0);
        thtm = atan2(-1.0+(2.0*g+1.0)/nd, 1.0);
        thte = atan2(1.0, 1.0-1.0/nd);
      else
        thts = atan2(-1.0+(2.0*g-1.0)/nd, 1.0);
        thtm = atan2(-1.0+(2.0*g+1.0)/nd, 1.0);
        thte = atan2(-1.0+(2.0*g+3.0)/nd, 1.0);
      end
      %fprintf(1,'%d %d %d\n',thts,thtm,thte);
      R21 = R2/R1;
      wpdata = fft2(cs{cnt}) / sqrt(numel(cs{cnt}));
      cnt = cnt+1;
      for xcur=ceil(xs):xe
        yfm = ceil( max([-R2, R21*xcur*tan(thts)]) );
        yto = floor( min([R2, R21*xcur*tan(thte)]) );
        ycur = yfm:yto;
        thtcur = atan2(ycur/R2,xcur/R1);
        [al,ar] = cvwindow((thtcur-thts)/(thtm-thts));
        [bl,br] = cvwindow((thtcur-thtm)/(thte-thtm));
        pou = al.*br;
        fh(mod(xcur,M1)+1,mod(ycur,M2)+1) = fh(mod(xcur,M1)+1,mod(ycur,M2)+1) + wpdata(mod(xcur,xn)+1,mod(ycur,yn)+1) .* pou;
      end
    end

    for f=nd-1:-1:nd/2
      ys = R2/4-(W2/2)/4;		  ye = R2;
      xs = -R1 + (2*f-1)*W1/2;		  xe = -R1 + (2*f+3)*W1/2;
      xn = ceil(xe-xs);		  yn = ceil(ye-ys);
      if(f==0)
        phis = atan2(-1.0, 1.0-1.0/nd);
        phim = atan2(-1.0+1.0/nd, 1.0);
        phie = atan2(-1.0+3.0/nd, 1.0);
      elseif(f==nd-1)
        phis = atan2(-1.0+(2.0*f-1.0)/nd, 1.0);
        phim = atan2(-1.0+(2.0*f+1.0)/nd, 1.0);
        phie = atan2(1.0, 1.0-1.0/nd);
      else
        phis = atan2(-1.0+(2.0*f-1.0)/nd, 1.0);
        phim = atan2(-1.0+(2.0*f+1.0)/nd, 1.0);
        phie = atan2(-1.0+(2.0*f+3.0)/nd, 1.0);
      end
      %fprintf(1,'%d %d %d\n',phis,phim,phie);
      R12 = R1/R2;
      wpdata = fft2(cs{cnt}) / sqrt(numel(cs{cnt}));
      cnt = cnt+1;
      for ycur=ceil(ys):ye
        xfm = ceil( max([-R1, R12*ycur*tan(phis)]) );
        xto = floor( min([R1, R12*ycur*tan(phie)]) );
        xcur = xfm:xto;
        phicur = atan2(xcur/R1, ycur/R2);
        [al,ar] = cvwindow((phicur-phis)/(phim-phis));
        [bl,br] = cvwindow((phicur-phim)/(phie-phim));
        pou = al.*br;
        fh(mod(xcur,M1)+1,mod(ycur,M2)+1) = fh(mod(xcur,M1)+1,mod(ycur,M2)+1) + wpdata(mod(xcur,xn)+1,mod(ycur,yn)+1) .* pou';
      end
    end

    %put back into fd
    fd(mod(idx1,A1)+1,mod(idx2,A2)+1) = fd(mod(idx1,A1)+1,mod(idx2,A2)+1) + pass .* fh(mod(idx1,M1)+1,mod(idx2,M2)+1);
  end

  if(1)
    s = 1;
    R1 = 2^(s-ns)*G1;
    R2 = 2^(s-ns)*G2;
    idx1 = [ceil(-R1):floor(R1)];
    [wl,wr] = cvwindow((idx1+R1/1)/(R1/2));    tmpa = wl;
    [wl,wr] = cvwindow((idx1-R1/2)/(R1/2));    tmpb = wr;
    coef1 = tmpa.*tmpb;
    idx2 = [ceil(-R2):floor(R2)];
    [wl,wr] = cvwindow((idx2+R2/1)/(R2/2));    tmpa = wl;
    [wl,wr] = cvwindow((idx2-R2/2)/(R2/2));    tmpb = wr;
    coef2 = tmpa.*tmpb;
    pass = coef1'*coef2;
    [M1,M2] = size(pass);

    cs = c{s};
    tmp = fft2(cs{1}) / sqrt(numel(cs{1}));
    tmp = tmp/4;
    tmp = mescatter(tmp,0);
    tmp = mescatter(tmp',0)';
    [K1,K2] = size(tmp);
    fh = zeros(M1,M2);
    fh(mod(idx1,M1)+1,mod(idx2,M2)+1) = tmp(mod(idx1,K1)+1,mod(idx2,K2)+1);

    fd(mod(idx1,A1)+1,mod(idx2,A2)+1) = fd(mod(idx1,A1)+1,mod(idx2,A2)+1) + pass .* fh(mod(idx1,M1)+1,mod(idx2,M2)+1);
  end

  fd = mecombine(fd,E1);
  fd = mecombine(fd',E2)';

  x = idct2(fd);


%% Basic Usage
% This document introduces the basic usage of MaxwellFDFD with example codes.

%% Example Code
% We start with a simple example code that solves a 2D problem, which is to
% examine the transmission of a plane wave through a narrow slit:
[E, H] = maxwell_run(...
	'OSC', 1e-9, 1550, ...
	'DOM', {'vacuum', 'none', 1.0}, [-1100 1100; -1100 2600; 0 10], 10, BC.p, [100 100 0],...
	'OBJ', {'CRC/Ag', 'k'}, ...
		Box([-1100 -80; 0 1000; 0 10]), ...
		Box([80 1100; 0 1000; 0 10]), ...
	'SRCJ', PlaneSrc(Axis.y, -500, Axis.x));

%%%
% The above is actually one line of code; the ellipsis |...| at the end of each
% line is MATLAB's way of indicating the continuation of a line.  Upon
% execution, this code calculates the solution _E_- and _H_-fields of the
% problem.  The solutions are stored in the variables |E| and |H|, which are the
% outputs of <../comp/maxwell_run.html |maxwell_run|>, MaxwellFDFD's core
% function, that takes a long list of arguments describing the problem.

%% Visualizing the Solution
% Before discussing the details of the arguments of |maxwell_run|, let's first
% visualize the solution to understand the problem we are solving better. This
% can be done by another simple line of code:
vis2d(E{Axis.x}, Axis.z, 5);

%%%
% which produces a figure that looks like:
%
% <<../img/basic_01.png>>
%
% Note that the PML regions are excluded from the plot by default.

%% Visualizing of Objects and Sources
% The above field plot would have been much more informative if it was overlaid
% with the objects and source placed in the simulation domain.  The arrays of
% the objects and sources can be obtained from |maxwell_run| by changing the
% output arguments as:
[E, H, obj_array, src_array] = maxwell_run({ARGUMENTS});

%%%
% These objects and sources (actually only one source for the present problem)
% are drawn on top of the field plot when they are supplied to |vis2d| as extra
% input arguments as
vis2d(E{Axis.x}, Axis.z, 5, obj_array, src_array);

%%%
% The modified code will produce an updated figure that looks like:
%
% <<../img/basic_02.png>>
%
% The black lines indicate the boundaies of the metal pieces, and green line
% indicates the location of the source plane.

%% Input Arguments of |maxwell_run|
% |maxwell_run| takes many input arguments that are grouped into several
% *parameter groups*.  Each parameter group is specified by |'NAME'| (such as
% |'OSC'|, |'DOM'|, |'OBJ'|, |'SRCJ'|) that is followed by the arguments in the
% parameter group.  Below, the meanings of the arguments used in the above
% example code are explained for each parameter group.
%
% *'OSC'* specifies the oscillation parameter group, which describes the
% wavelength of the source.
%
% * |1e-9|: the unit of wavelength is 1 nm (= 1e-9 m).
% * |1550|: the wavelength is 1550 nm.
%
% The unit of wavelength specified in this parameter group serves as the unit of
% all the length arguments of |maxwell_run|.
%
% *'DOM'* specifies the domain parameter group, which describes the simulation
% domain of the problem.
%
% * |{'vacuum', 'none', 1.0}|: the background material that fills the simulation
% domain is named |'vacuum'| (just a user-defined name), will be visualized with
% color |'none'| (i.e., the material will not be shown in the figure generated
% by |vis2d|), and its dielectric constant is 1.0.
% * |[-1100 1100; -1100 2600; 0 10]|: the simulation domain lies between -1100
% and 1100 nm in the _x_-direction, -1100 and 2600 nm in the _y_-direction, 0
% and 10 nm in the _z_-direction.
% * |10|: the coarsest grid size used to descritize the simulation domain is 10
% nm.
% * |BC.p|: periodic boundary conditions are used in all the _x_-, _y_-,
% _z_-directions.
% * |[100 100 0]|: the thicknesses of PML are 100 nm at the _x_- and _y_-normal
% boundaries, and no PML is used at the _z_-normal boundaries.  The simulation
% domain except PML lies between -1000 and 1000 nm in the _x_-direction, -1000
% and 2500 nm in the _y_-direction, 0 and 10 nm in the _z_-direction.
%
% *'OBJ'* specifies the object parameter group, which describes the objects
% placed in the simulation domain.
%
% * |{'CRC/Ag', 'k'}|: the material used to create objects has
% frequency-dependent dielectric constants defined in the file |Ag.mat| under
% the subdirectory |dielconst/CRC/| under the main MaxwellFDFD directory.  (In
% fact, the dielectric constants are taken from the silver data in the CRC
% handbook.)  When the objects made of this material are plotted, they will be
% colored black (whose MATLAB color code is |'k'|).
% * |Box([-1100 -80; 0 1000; 0 10])|: place a box made of the material.  The
% box lies between -1100 and -80 nm in the _x_-direction, 0 and 1000 nm in
% the _y_-direction, 0 and 10 nm in the _z_-direction.
% * |Box([80 1100; 0 1000; 0 10])|: place another box made of the material.
%
% *'SRCJ'* specifies the _J_ source parameter group, which describes the
% electric current sources placed in the simulation domain.
%
% * |PlaneSrc(Axis.y, -500, Axis.x))|: at _y_ = -500 nm, place a plane of
% electric dipoles oscillating in the _x_-direction.
%
% Note that the order of appearance of the parameter groups in |maxwell_run| is
% interchangeable; for example, you can put the source parameter group before
% the object parameter group, and vice versa.

%% Inspecting the Design before Solution
% The above example is simple, so it does not hurt to solve the problem without
% making sure that the system you described with the arguments of |maxwell_run|
% is indeed what you intended to examine.  However, for larger and more complex
% problems, the solution process can be time-consuming.  Therefore, in general
% it is a good practice to inspect the design before solving the problem.
%
% You can command |maxwell_run| to visualize the simulation domain without
% solving the problem by appending a logical argument (|true| or |false|) as:
[E, H, obj_array, src_array] = maxwell_run({PARAMETER GROUPS}, true);

%%%
% The above code does not return solution _E_- and _H_-fields in the output
% arguments |E| and |H|, i.e., |E| and |H| are empty, but it produces the
% following figure:
%
% <<../img/basic_03.png>>
%
% The above figure visualizes the objects and source in the simulation domain,
% including the PML regions.

%% Complete Code
% Below is the complete code that has everything discussed in this document.
% Note that |vis2d| works only if the last argument of |maxwell_run| is flipped
% to |false| to calculate the solution.
[E, H, obj_array, src_array] = maxwell_run(...
	'OSC', 1e-9, 1550, ...
	'DOM', {'vacuum', 'none', 1.0}, [-1100 1100; -1100 2600; 0 10], 10, BC.p, [100 100 0],...
	'OBJ', {'CRC/Ag', 'k'}, ...
		Box([-1100 -80; 0 1000; 0 10]), ...
		Box([80 1100; 0 1000; 0 10]), ...
	'SRCJ', PlaneSrc(Axis.y, -500, Axis.x), ...
	false);  % true to inspect arguments without solving equation

vis2d(E{Axis.x}, Axis.z, 5, obj_array, src_array);

%%% See Also
% <intermediate.html |Intermediate Usage|>, <../comp/maxwell_run.html |maxwell_run|>