% [INPUT]
% p = A vector of floats [0,Inf) of length t representing the prices.
% bw = An integer [21,252] representing the dimension of each rolling window.
% w = An integer [500,Inf) representing the number of sweeps (optional, default=1000).
% c = A float (0,Inf) representing the starting coefficient value (optional, default=0.01).
% s2 = A float (0,Inf) representing the starting variance of innovations (optional, default=0.0004).
%
% [OUTPUT]
% ris = A column vector of floats (0,Inf) of length t representing the Roll implicit spread.

function ris = roll_implicit_spread(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('p',@(x)validateattributes(x,{'double'},{'real' 'finite' 'nonnegative' 'vector' 'nonempty'}));
        ip.addRequired('bw',@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 21 '<=' 252 'scalar'}));
        ip.addOptional('w',1000,@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 500}));
        ip.addOptional('c',0.01,@(x)validateattributes(x,{'double'},{'real' 'finite' 'positive'}));
        ip.addOptional('s2',0.0004,@(x)validateattributes(x,{'double'},{'real' 'finite' 'positive'}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    p = validate_input(ipr.p);
    bw = ipr.bw;
    w = ipr.w;
    c = ipr.c;
    s2 = ipr.s2;

    nargoutchk(1,1);

    ris = roll_implicit_spread_internal(p,bw,w,c,s2);

end

function ris = roll_implicit_spread_internal(p,bw,w,c,s2)

    up = isempty(getCurrentTask());

    windows = extract_rolling_windows(log(max(1e-6,p)),bw);
    ris = zeros(numel(windows),1);

    if (up)
        parfor i = 1:numel(windows)
            ris(i) = gibbs_sampler(windows{i},w,c,s2);
        end
    else
        for i = 1:numel(windows)
            ris(i) = gibbs_sampler(windows{i},w,c,s2);
        end
    end

    alpha = 2 / (bw + 1);

    ris = [ris(1); filter(alpha,[1 (alpha - 1)],ris(2:end),(1 - alpha) * ris(1))];

end

function g = gibbs_sampler(p,w,c,s2)

    dp = diff(p);
    q = [1; sign(dp)];

    for i = 1:w
        dq = diff(q);

        d = 1 + ((1 / s2) * (dq.' * dq));
        mu = linsolve(d,(1 / s2) * (dq.' * dp));
        rho = inv(d);
        c = mvnrnd_truncated(mu,rho);

        u = dp - (c .* dq);
        alpha = 1e-12 + (numel(u) / 2);
        beta = 1e-12 + (sum(u.^2) / 2);
        s2 = 1 / ((1 / beta) * gamrnd(alpha,1));

        q = perform_draw(p,q,c,s2);
    end

    g = 2 * c;

end

function q = perform_draw(p,q,c,s2)

    t = numel(p);
    q_nnz = q ~= 0;
    s22 = s2 * 2;

    m = mod((1:t).',2);
    r = rand(t,1);

    q = [q q];
    p = [p p];

    for i = 1:2
        o = (m == (i - 1)) & q_nnz;

        if (~any(o))
            continue;
        end

        q(o,1) = 1;
        q(o,2) = -1;

        u = diff(p - (c * q));

        s = u.^2 ./ s22;
        s_sum = [s; [0 0]] + [[0 0]; s];
        s_sum = s_sum(o,:);

        odds_log = diff(s_sum,1,2);
        in_range = odds_log < 500;
        odds = exp(in_range .* odds_log);

        buy = odds ./ (1 + odds);
        buy = (in_range .* buy) + ~in_range;

        if (i == 1)
            q(o,:) = repmat(1 - (2 * (r(o) > buy)),1,2);
        else
            q(o,1) = 1 - (2 * (r(o) > buy));
        end
    end

    q = q(:,1);

end

function r = mvnrnd_truncated(mu,rho)

    f = sqrt(rho);
    low = -mu / f;

    if (low > 6)
        eta = low + (100 * eps());
    else
        plow = normcdf(low);

        p = plow + (rand() * (1 - plow));

        if (p == 1)
            eta = low + (100 * eps());
        else
            eta = norminv(p);
        end
    end

    r = mu + (f * eta);

end

function p = validate_input(p)

    p = p(:);

    if (numel(p) < 5)
        error('The value of ''p'' is invalid. Expected input to be a vector containing at least 5 elements.');
    end

end


function [Image,x0,y0] = move_circle2(Image,I_ellipse,xlo,xhi,ylo,yhi,ix,iy);

image_size = length(Image);

while max(max(Image(ix,iy))) > 1

    Image(ix,iy) = Image(ix,iy) - I_ellipse;

    x0 = ceil(rand*image_size);
    y0 = ceil(rand*image_size);

    nlo = x0 - xlo; nhi = x0 + xhi;
    ix = mod(nlo:nhi,length(Image));ix(ix==0)=length(Image);
    nlo = y0 - ylo; nhi = y0 + yhi;
    iy = mod(nlo:nhi,length(Image));iy(iy==0)=length(Image);

    Image(ix,iy) = Image(ix,iy) + I_ellipse;

end


function [xDwCentr,yDwCentr,zDwCentr] = calc_doseWeightedCentroid(structNum,doseNum,normalizeFlag,planC)
%function [xDwCentr,yDwCentr,zDwCentr] = calc_doseWeightedCentroid(structNum,doseNum,normalizeFlag,planC)
%
% This function returns the dose-weighted centroid for the input structure and dose.
%
% INPUTS:
% structNum: structure index within the planC object
% doseNum: dose index within the planC object
% normalizeFlag: Flag to normalize the centroid between 0 and 1.
% If normalizeFlag == 1, the structure coordinates are scaled between and 0
% and 1 based on min/max coordinates along that dimension. x,y,z are based
% on RTOG coordinate system: https://github.com/cerr/CERR/wiki/Coordinate-system
% planC: can be accessed from Viewer by using 'global planC' or by loading
% the CERR .mat file in memory.
%
% OUTPUTS:
% x,y,z: dose weighted centroid coordinates
%
% Example:
%
% structNum = 10;
% doseNum = 1;
% normFlg = 1;
% global planC
% [x,y,z] = calc_doseWeightedCentroid(structNum,doseNum,normFlg,planC)
%
%APA, 05/15/2018
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
% CERR is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
% without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
% See the GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with CERR.  If not, see <http://www.gnu.org/licenses/>.

%Check if plan passed, if not use global.
if ~exist('planC')
    global planC;
end
indexS = planC{end};

[iV,jV,kV] = getUniformStr(structNum, planC);
assocScanNum = getStructureAssociatedScan(structNum,planC);
[xV,yV,zV] = getUniformScanXYZVals(planC{indexS.scan}(assocScanNum));
xCalcV = xV(jV);
yCalcV = yV(iV);
zCalcV = zV(kV);

%Get scan transM
transM_scan = getTransM('scan',assocScanNum,planC);
if isempty(transM_scan)
    transM_scan = eye(4);
end

%Get dose transM
transM_dose = getTransM('dose',doseNum,planC);
if isempty(transM_dose)
    transM_dose = eye(4);
end

%Apply transM to dose calc pts
if ~isequal(transM_scan,transM_dose)
    [xCalcV, yCalcV, zCalcV] = applyTransM(transM_scan*inv(transM_dose), xCalcV, yCalcV, zCalcV);
    [jnk,jnk,zV] = applyTransM(transM_scan*inv(transM_dose), zV*0, zV*0 ,zV);
end

dosesV = getDoseAt(doseNum, xCalcV, yCalcV, zCalcV, planC);

%Normalize x,y,z coordinates
if normalizeFlag
    xCalcV = (xCalcV - min(xCalcV)) ./ (max(xCalcV) - min(xCalcV));
    yCalcV = (yCalcV - min(yCalcV)) ./ (max(yCalcV) - min(yCalcV));
    zCalcV = (zCalcV - min(zCalcV)) ./ (max(zCalcV) - min(zCalcV));
end

xDwCentr = sum(dosesV.*xCalcV)/sum(dosesV);
yDwCentr = sum(dosesV.*yCalcV)/sum(dosesV);
zDwCentr = sum(dosesV.*zCalcV)/sum(dosesV);


function S3G = equispacedSO3Grid(CS,SS,varargin)
% defines a equispaced grid in the orientation space
%
% Syntax
%   S3G = equispacedSO3Grid(CS,SS,'points',n)
%   S3G = equispacedSO3Grid(CS,'resolution',res)
%
%   % fill only a ball with radius of 20 degree
%   S3G = equispacedSO3Grid(CS,'maxAngle',20*degree)
%
% Input
%  CS  - @crystalSymmetry
%  SS  - @specimenSymmetry
%   n  - approximate number of points
%  res - resolution in radiant
%
% Output
%  S3G - @SO3Grid
%
% Options
%  maxAngle - radius of the ball to be filles
%  center - center of the ball
%
% See also
% equispacedS2Grid, SO3Grid/SO3Grid

% extract specimen symmetry if provided
if nargin == 1
  SS = specimenSymmetry('1');
elseif ~isa(SS,'symmetry')
  varargin = [{SS},varargin];
  SS = specimenSymmetry('1');
end

% may be we should populate only a ball
maxAngle = get_option(varargin,'maxAngle',2*pi);

if maxAngle < pi/2/CS.multiplicityZ
  S3G = localOrientationGrid(CS,SS,maxAngle,varargin{:});
  return
end

% get fundamental region
[maxAlpha,maxBeta,maxGamma] = fundamentalRegionEuler(CS,SS,'SO3Grid',varargin{:});
maxGamma = maxGamma/2; % we will consider the interval -maxGamma/2 .. maxGamma/2
if ~check_option(varargin,'center'), maxGamma = min(maxGamma,maxAngle);end

% determine resolution
if check_option(varargin,'points')

  points = get_option(varargin,'points');

  switch CS.LaueName  % special case: cubic symmetry
    case 'm-3'
      points = 3*points;
    case 'm-3m'
      points = 2*points;
  end

  % calculate number of subdivisions for the angles alpha,beta,gamma
  res = 2/(points/( maxBeta*maxGamma))^(1/3);

  if  maxAngle < pi * 2 && maxAngle < maxBeta
    res = res * maxAngle; % bug: does not work properly for all syms
  end

else

  res = get_option(varargin,'resolution',5*degree);

end

alphabeta = equispacedS2Grid('resolution',res,...
  'maxTheta',maxBeta,'minRho',0,'maxRho',maxAlpha,...
  no_center(res),'restrict2minmax');

ap2 = round(2*maxGamma/res);

[beta,alpha] = polar(alphabeta);

% calculate gamma shift
re = cos(beta).*cos(alpha) + cos(alpha);
im = -(cos(beta)+1).*sin(alpha);
dGamma = atan2(im,re);
dGamma = repmat(reshape(dGamma,1,[]),ap2,1);
gamma = -maxGamma + (0:ap2-1) * 2 * maxGamma / ap2;

% arrange alpha, beta, gamma
gamma  = dGamma+repmat(gamma.',1,length(alphabeta));
alpha = repmat(reshape(alpha,1,[]),ap2,1);
beta  = repmat(reshape(beta,1,[]),ap2,1);

ori = orientation.byEuler(alpha,beta,gamma,'ZYZ',CS,SS,varargin{:});

gamma = S1Grid(gamma,-maxGamma+dGamma(1,:),...
  maxGamma+dGamma(1,:),'periodic','matrix');

res = 2 * maxGamma / ap2;

% eliminiate 3 fold symmetry axis of cubic symmetries
% TODO: this should be done better!!
ind = fundamental_region(ori,CS,specimenSymmetry);

if nnz(ind) ~= 0
  % eliminate those rotations
  ori(ind) = [];

  % eliminate from index set
  gamma = subGrid(gamma,~ind);
  alphabeta  = subGrid(alphabeta,GridLength(gamma)>0);
  gamma(GridLength(gamma)==0) = [];

end

S3G = SO3Grid(ori,alphabeta,gamma,'resolution',res);

if check_option(varargin,'maxAngle')
  center = get_option(varargin,'center',quaternion.id);
  S3G = subGrid(S3G,center,maxAngle);
end

end

% ----------------------------------------------------------------
function s = no_center(res)

if mod(round(2*pi/res),2) == 0
  s = 'no_center';
else
  s = '';
end
end

% ---------------------------------------------------------------
function ind = fundamental_region(q,cs,ss)

if isempty(q), ind = []; return; end

c = {};

% eliminiate 3 fold symmetry axis of cubic symmetries
switch cs.LaueName

  case   {'m-3m','m-3'}

    c{end+1}.v = vector3d([1 1 1 1 -1 -1 -1 -1],[1 1 -1 -1 1 1 -1 -1],[1 -1 1 -1 1 -1 1 -1]);
    c{end}.h = sqrt(3)/3;

    if strcmp(cs.LaueName,'m-3m')
      c{end+1}.v = vector3d([1 -1 0 0 0 0],[0 0 1 -1 0 0],[0 0 0 0 1 -1]);
      c{end}.h = sqrt(2)-1;
    end
end

switch ss.LaueName
  case 'mmm'
   c{end+1}.v = vector3d([-1 0],[0 -1],[0 0]);
   c{end}.h = 0;
end

% find rotation not part of the fundamental region
rodrigues = Rodrigues(q); clear q;
ind = false(length(rodrigues),1);
for i = 1:length(c)
  for j = 1:length(c{i}.v)
    p = dot(rodrigues,1/norm(c{i}.v(j)) * c{i}.v(j));
    ind = ind | (p(:)>c{i}.h);
  end
end

end


function s = zeroSingFun()
%ZEROSINGFUN   Constructs the zero SINGFUN. The output SINGFUN object has a
%   zero smooth part with no singularities at any end points.
%
% See also SINGFUN.

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

% Create an empty SINGFUN object:
s = singfun();

% Create a zero smooth part:
s.smoothPart = singfun.constructSmoothPart(0, [], []);

% No singularities at any end points:
s.exponents = [0, 0];

end


function bb2()
% BB2 Bouncing Smiley Ball Physics
% BB2 is a graphical User Interface that creates a simple simulation of a ball bouncing
% off the ground.
%
% Ball Parameters are:
%
% Height from the center of the ball to the ground is 10. This is constant and cannot
% change. Instead, change the other parameters since they're all relative.
%
% Radius: Radius of the ball. It's best to put it between 1 and 9. Any value equal to
% or above 10 will present problems as 10 is the distance from the centre of the ball
% to the ground.
%
% Gravity: Affect the ratio in which the ball gains speed in the negative Y direction.
%
% Initial Yv: Initial velocity in the Y direction. Can be positive, negative, or anything.
%
% Initial Xv: Initial velocity in the X direction. Can be positive, negative, or anything.
%
% Vertical Speed Conservation (%): Affects how much energy (velocity) is retained in the
% Y direction after hitting the ground. A value of 0 to 100 is normal. A value of 0 means
% no energy is retained and the ball comes to a halt after hitting the ground. A value
% of 100 means all energy is retained and the ball will bounce indefinitely. A value of
% over 100 will cause energy to increase after hitting the ground.
%
% Horizontal Speed Conservation (%): Same as Vertical Speed Conservation but in the
% X direction.
%
% Initial Angular Velocity (CW): The initial rotational velocity of the ball (spin).
%
% Plotting a path enables you to see the x-y diagram of the ball's movement and gives
% you the ability to pan the axes with the mouse.
%
%
% Please note that the ball may stop moving at different locations for different
% trials, even with the same input parameters. This is due to the slight
% randomization of the deformations of the ball and the ground, which could lead
% to the ball getting "stuck" to the ground at times. This is normal and
% deliberate.
%
% Set the vertical speed conservation as a value larger than 100 (e.g. 170) for
% a small bonus ;)
%
% I'd like to thank John D'Errico and Kennith Eaton whose ideas helped me a lot.
%
%
% To launch the GUI, type bb2 in the command window with this file in the current
% directory. Alternatively, you can choose Debug -> Run from this editor window, or
% press F5.
%
% Tested on MATLAB 7.4 (R2007a)
%
% Husam Aldahiyat, 2009
% numandina@gmail.com
%

figure('units','normalized','position',[.2 .2 .65 .65],'menubar','none','numbertitle','off','color','w','name','Bouncing Smiley')
axes('position',[.25 .05 .65 .75])
ed1=uicontrol('style','edit','units','normalized','position',[.025 .895 .1 .05],'backgroundcolor','w','string','1','callback',@init);
uicontrol('style','text','units','normalized','position',[.025 .96 .1 .025],'backgroundcolor','w','string','Radius');
ed2=uicontrol('style','edit','units','normalized','position',[.025 .775 .1 .05],'backgroundcolor','w','string','9.81');
uicontrol('style','text','units','normalized','position',[.025 .84 .1 .025],'backgroundcolor','w','string','Gravity');
ed3=uicontrol('style','edit','units','normalized','position',[.025 .65 .1 .05],'backgroundcolor','w','string','0');
uicontrol('style','text','units','normalized','position',[.025 .72 .1 .025],'backgroundcolor','w','string','Initial Yv');
ed4=uicontrol('style','edit','units','normalized','position',[.025 .535 .1 .05],'backgroundcolor','w','string','500');
uicontrol('style','text','units','normalized','position',[.025 .6 .1 .025],'backgroundcolor','w','string','Initial Xv');
ed5=uicontrol('style','edit','units','normalized','position',[.025 .415 .1 .05],'backgroundcolor','w','string','70');
uicontrol('style','text','units','normalized','position',[.005 .48 .21 .025],'backgroundcolor','w','string',...
	'Vertical Speed Conservation (%)','horizontalalignment','left');
ed6=uicontrol('style','edit','units','normalized','position',[.025 .295 .1 .05],'backgroundcolor','w','string','50');
uicontrol('style','text','units','normalized','position',[.005 .36 .21 .025],'backgroundcolor','w','string',...
	'Horizontal Speed Conservation (%)','horizontalalignment','left');
ed7=uicontrol('style','edit','units','normalized','position',[.025 .175 .1 .05],'backgroundcolor','w','string','25');
uicontrol('style','text','units','normalized','position',[.005 .24 .21 .025],'backgroundcolor','w','string',...
	'Initial Angular Velocity (CW)','horizontalalignment','left');

tb=uicontrol('style','togglebutton','string','Start','callback',@go,'units','normalized','position',[.025 .05 .1 .05],...
	'backgroundcolor','g');
chk=uicontrol('style','checkbox','string','Plot Path','units','normalized','position',[.025 .125 .1 .025],'backgroundcolor','w');
dummy1=uicontrol('style','text','max',2,'visible','off');
init;

	function [hb,h2,hx,h1,r,t,KE,hL]=init(varargin)
		r=str2double(get(ed1,'string'));
		t=linspace(0,2*pi,100);
		cla reset
		hold on
		h1=fill((5)+r*cos(t),(10)+r*sin(t),'y');
		am =[
			0    0    0    0    0    1    1    0    0    0    0    0    0    0    0    0    0    1    1    0    0    0    0    0    0
			0    0    0    0    1    1    1    1    0    0    0    0    0    0    0    0    1    1    1    1    0    0    0    0    0
			0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    0    0    0    0    0
			0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
			0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
			0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
			0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
			0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
			0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
			0    0    0    0    1    1    1    1    0    0    0    0    0    0    0    0    1    1    1    1    0    0    0    0    0
			0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0
			1    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    1    1
			1    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    1    1
			1    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    1    1
			1    1    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    1    1    1
			1    1    1    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    1    1    1
			1    1    1    1    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    1    1    1    1
			0    0    1    1    1    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    1    1    1    0    0
			0    0    0    0    1    1    1    0    0    0    0    0    0    0    0    0    0    0    0    1    1    1    0    0    0
			0    0    0    0    0    1    1    1    1    1    1    1    1    1    1    1    1    1    1    1    0    0    0    0    0
			0    0    0    0    0    0    0    1    1    1    1    1    1    1    1    1    1    1    1    0    0    0    0    0    0
			];
		am=[zeros(2,25);am;zeros(2,25)];
		colormap([1 1 0;0 0 0]);
		axis([0 10 -1 10+r+1])
		axis equal
		hx=axis;
		hL=line(1000.*[hx(1),hx(2)],[10 10]);
		axis(hx)
		set(hL,'color','c','linestyle','--')
		set(h1,'linewidth',3)
		hL=line(1000.*[hx(1) hx(2)],[0 0]);
		set(hL,'color','k','linestyle','-','linewidth',3)
		h2=plot(5,10,'k+');
		hb=[h1,h2];
		KE=surface(flipud(am),'facecolor','texturemap','xdata',linspace(get(h2,'xdata')-r/2,get(h2,'xdata')+r/2,...
				25),'ydata',linspace(get(h2,'ydata')-r/2,get(h2,'ydata')+r/2,25),'edgecolor','none');
	end

	function go(varargin)
		set(tb,'value',0)
		if strcmp(get(tb,'string'),'Start')
			set(tb,'string','Stop','backgroundcolor','r')
		else
			set(tb,'string','Start','backgroundcolor','g')
			return
		end
		set(dummy1,'string','')
		dt=1;
		[hb,h2,hx,h1,r,t,KE]=init;
		rot=str2double(get(ed7,'string'));
		a=str2double(get(ed2,'string'))/2000;
		Yv=-1*str2double(get(ed3,'string'))/2000;
		Xv=str2double(get(ed4,'string'))/2000;
		e=str2double(get(ed5,'string'))/100;
		f=str2double(get(ed6,'string'))*.7/100;
		pc=0;
		c=0;
		set(h2,'visible','off')
		ll=0;
		while strcmp(get(tb,'string'),'Stop')
			if c<5
				am =[
				0    0    0    0    0    1    1    0    0    0    0    0    0    0    0    0    0    1    1    0    0    0    0    0    0
				0    0    0    0    1    1    1    1    0    0    0    0    0    0    0    0    1    1    1    1    0    0    0    0    0
				0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    0    0    0    0    0
				0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
				0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
				0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
				0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
				0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
				0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
				0    0    0    0    1    1    1    1    0    0    0    0    0    0    0    0    1    1    1    1    0    0    0    0    0
				0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0
				1    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    1    1
				1    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    1    1
				1    1    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    1    1
				1    1    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    1    1    1
				1    1    1    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    1    1    1
				1    0    1    1    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    1    1    0    1
				0    0    0    1    1    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    1    1    1    0    0
				0    0    0    0    1    1    1    0    0    0    0    0    0    0    0    0    0    0    0    1    1    1    0    0    0
				0    0    0    0    0    1    1    1    1    1    1    1    1    1    1    1    1    1    1    1    0    0    0    0    0
				0    0    0    0    0    0    0    1    1    1    1    1    1    1    1    1    1    1    1    0    0    0    0    0    0
				];
			am=[zeros(2,25);am;zeros(2,25)]; %#ok
			colormap([1 1 0;0 0 0])
			set(h1,'facecolor','y')
			p=0;
			else
				c=c-1;
				p=p+1;
			end
			if p>30 && rot<.1
				set(tb,'string','Start','backgroundcolor','g');
				break
			end
			pc=pc+1;
			yc=get(h2,'ydata');
			if get(chk,'value')
				plot(get(h2,'xdata'),yc,'k.')
				pan on
			end
			Yv=Yv+a*dt;
			if yc-(Yv+a*dt)*dt<=r-min(get(h1,'ydata'))
				if Yv>1
					figure('units','normalized','menubar','none','numbertitle','off','color','w')
					set(gcf,'position',[0 -.3 1 1.27])
					axes('position',[0 0 1 1 ])
					t=-4:.01:4;
					sound(cos(300.*t.^2));
					a=imread('face.jpg');
					image(a)
					return
				end
				Yv=Yv+a*dt/(Yv+a*dt)*(yc-r);
				Yv=-Yv*e;
				yt=get(h1,'ydata');
				xxt=get(h1,'xdata');
				ys=sort(yt);
				xr=xxt((ismember(yt,ys(1:10)))==1);
				xrp=str2num(get(dummy1,'string')); %#ok
				set(dummy1,'string',mat2str(xr));
				Xv=Xv*f;
				rot2=rot/10+Xv/r*100;
				Xv=Xv+rot/250;
				rot=rot2;
				% rot=sqrt(10*a*hg/7/r^2)*10;
				if ~isempty(xrp) && sign(Xv+~sign(Xv))*max(xrp)>min(xr)*sign(Xv+~sign(Xv))
					newy=get(pl,'ydata')-rand(size(get(pl,'ydata')))/10*abs(Yv);
					newy(1)=0;
					newy(end)=0;
					set(pl,'ydata',newy);
				else
					sar=zeros(1,8)-(rand(1,8).*(1-f).*abs(Yv));
					ltt=line([min(xr) max(xr)],[0 0]);
					set(ltt,'linewidth',3,'color','w')
					pl=plot(sort(xr),[zeros(1,length(xr)-9),sar,0]);
					set(pl,'linewidth',2.5,'color','k')
				end
 				yt((ismember(yt,ys(1:10))))=yt((ismember(yt,ys(1:10))))+...
					transpose(rand(1,length(yt((ismember(yt,ys(1:10)))))).*(1-f).*abs(Yv));
				set(h2,'ydata',r)
				set(h1,'ydata',yt)
				am =[
					0    0    0    0    0    1    1    0    0    0    0    0    0    0    0    0    0    1    1    0    0    0    0    0    0
					0    0    0    0    1    1    1    1    0    0    0    0    0    0    0    0    1    1    1    1    0    0    0    0    0
					0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
					0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
					0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
					0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
					0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
					0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
					0    0    0    1    1    1    1    1    1    0    0    0    0    0    0    1    1    1    1    1    1    0    0    0    0
					0    0    0    0    1    1    1    1    0    0    0    0    0    0    0    0    1    1    1    1    0    0    0    0    0
					0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0
					0    0    0    0    0    0    0    0    1    1    1    1    1    1    1    1    0    0    0    0    0    0    0    0    0
					0    0    0    0    0    0    0    0    1    1    1    1    1    1    1    1    0    0    0    0    0    0    0    0    0
					0    0    0    0    0    0    0    1    1    1    0    0    0    0    1    1    1    0    0    0    0    0    0    0    0
					0    0    0    0    0    0    0    1    1    1    0    0    0    0    1    1    1    0    0    0    0    0    0    0    0
					0    0    0    0    0    0    0    1    1    0    0    0    0    0    0    1    1    0    0    0    0    0    0    0    0
					0    0    0    0    0    0    0    1    1    0    0    0    0    0    0    1    1    0    0    0    0    0    0    0    0
					0    0    0    0    0    0    0    1    1    0    0    0    0    0    0    1    1    0    0    0    0    0    0    0    0
					0    0    0    0    0    0    0    1    1    1    1    1    1    1    1    1    1    0    0    0    0    0    0    0    0
					0    0    0    0    0    0    0    1    1    1    1    1    1    1    1    1    1    0    0    0    0    0    0    0    0
					0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0
					];
				am=[zeros(2,25);am;zeros(2,25)]; %#ok
				colormap([1 0 0;0 0 0])
				set(h1,'facecolor','r')
				c=15;
			else
				for k=1:2
					set(hb(k),'ydata',get(hb(k),'ydata')-Yv*dt)
				end
			end
			set(KE,'xdata',get(KE,'xdata')+Xv*dt)
			set(hb(1),'xdata',get(hb(1),'xdata')+Xv*dt)
			set(hb(2),'xdata',get(hb(2),'xdata')+Xv*dt)
			l=axis;
			if get(h2,'xdata')>l(2)-r && Xv>0
				xlim([hx(1),hx(2)]+l(2))
			end
			if get(h2,'ydata')>l(4)-r && Yv<0
				ylim([hx(3),hx(4)]+l(4))
			end
			if get(h2,'xdata')<l(1)+r && Xv<0
				xlim([l(1),l(2)]-(hx(2)-hx(1)))
			end
			if get(h2,'ydata')<l(3)+r && Yv>0
				ylim([l(3),l(4)]-(hx(4)-hx(3)))
			end
			ccc=(max(get(h1,'ydata'))+min(get(h1,'ydata')))/2;
			ccc2=(max(get(h1,'xdata'))+min(get(h1,'xdata')))/2;
			ll=ll+rot;
			delete(KE)
			KE=surface(flipud(am),'facecolor','texturemap','xdata',linspace(ccc2-r/2,ccc2+r/2,...
				25),'ydata',linspace(ccc-r/2,ccc+r/2,25),'edgecolor','none');
			rotate(KE,[0 0 1],-ll,[ccc2 ccc 1])
			rotate(h1,[0 0 1],-rot,[ccc2,ccc 1])
			drawnow()
		end
	end
end


clear; close all; clc;
load '5jia1.mat'
load 'x1'
load 'x2'
load 'x3'
load 'x4'
load 'x5'
load 'uavW.mat'
i = uavW;
clear uavW;

P.gravity = 9.8;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Params for Aersonade UAV
%physical parameters of airframe
P.mass = 13.5;
P.Jx   = 0.8244;
P.Jy   = 1.135;
P.Jz   = 1.759;
P.Jxz  = .1204;
% aerodynamic coefficients
P.S_wing        = 0.55;
P.b             = 2.8956;
P.c             = 0.18994;
P.S_prop        = 0.2027;
P.rho           = 1.2682;
P.k_motor       = 80;
P.k_T_P         = 0;
P.k_Omega       = 0;
P.e             = 0.9;

P.C_L_0         = 0.28;
P.C_L_alpha     = 3.45;
P.C_L_q         = 0.0;
P.C_L_delta_e   = -0.36;
P.C_D_0         = 0.03;
P.C_D_alpha     = 0.30;
P.C_D_p         = 0.0437;
P.C_D_q         = 0.0;
P.C_D_delta_e   = 0.0;
P.C_m_0         = -0.02338;
P.C_m_alpha     = -0.38;
P.C_m_q         = -3.6;
P.C_m_delta_e   = -0.5;
P.C_Y_0         = 0.0;
P.C_Y_beta      = -0.98;
P.C_Y_p         = 0.0;
P.C_Y_r         = 0.0;
P.C_Y_delta_a   = 0.0;
P.C_Y_delta_r   = -0.17;
P.C_ell_0       = 0.0;
P.C_ell_beta    = -0.12;
P.C_ell_p       = -0.26;
P.C_ell_r       = 0.14;
P.C_ell_delta_a = 0.08;
P.C_ell_delta_r = 0.105;
P.C_n_0         = 0.0;
P.C_n_beta      = 0.25;
P.C_n_p         = 0.022;
P.C_n_r         = -0.35;
P.C_n_delta_a   = 0.06;
P.C_n_delta_r   = -0.032;
P.C_prop        = 1.0;
P.M             = 50;
P.epsilon       = 0.1592;
P.alpha0        = 0.4712;

% wind parameters
P.wind_n = 0;
P.wind_e = 0;
P.wind_d = 0;
P.L_u = 200;
P.L_v = 200;
P.L_w = 50;
P.sigma_u = 1.06;
P.sigma_v = 1.06;
P.sigma_w = .7;

% r1 - r8
P.r = P.Jx*P.Jz-P.Jxz^2;
P.r1 = P.Jxz*(P.Jx-P.Jy+P.Jz)/P.r;
P.r2 = P.Jz*(P.Jz-P.Jy)+P.Jxz^2;
P.r3 = P.Jz/P.r;
P.r4 = P.Jxz/P.r;
P.r5 = (P.Jz-P.Jx)/P.Jy;
P.r6 = P.Jxz/P.Jy;
P.r7 = ((P.Jx-P.Jy)*P.Jx+P.Jxz^2)/P.r;
P.r8 = P.Jx/P.r;

% C parameters on p.62
P.C_p_0 = P.r3 * P.C_ell_0 + P.r4 * P.C_n_0;
P.C_p_beta = P.r3 * P.C_ell_beta + P.r4 * P.C_n_beta;
P.C_p_p = P.r3 * P.C_ell_p + P.r4 * P.C_n_p;
P.C_p_r = P.r3 * P.C_ell_r + P.r4 * P.C_n_r;
P.C_p_delta_a = P.r3 * P.C_ell_delta_a + P.r4 * P.C_n_delta_a;
P.C_p_delta_r = P.r3 * P.C_ell_delta_r + P.r4 * P.C_n_delta_r;
P.C_r_0 = P.r4 * P.C_ell_0 + P.r8 * P.C_n_0;
P.C_r_beta = P.r4 * P.C_ell_beta + P.r8 * P.C_n_beta;
P.C_r_p = P.r4 * P.C_ell_p + P.r8 * P.C_n_p;
P.C_r_r = P.r4 * P.C_ell_r + P.r8 * P.C_n_r;
P.C_r_delta_a = P.r4 * P.C_ell_delta_a + P.r8 * P.C_n_delta_a;
P.C_r_delta_r = P.r4 * P.C_ell_delta_r + P.r8 * P.C_n_delta_r;

% compute trim conditions using 'mavsim_chap5_trim.slx'
% initial airspeed
P.Va0 = 35;
gamma = 0; %5*pi/180;  % desired flight path angle (radians)
R     = inf; %150;         % desired radius (m) - use (+) for right handed orbit, can't be 0

% autopilot sample rate
P.Ts = 0.01;
P.tau = 0.5;

% first cut at initial conditions

P.pn0    = 0;  % initial North position
P.pe0    = 0;  % initial East position
P.pd0    = 0;  % initial Down position (negative altitude)

P.u0     = P.Va0; % initial velocity along body x-axis
P.v0     = 0;  % initial velocity along body y-axis
P.w0     = 0;  % initial velocity along body z-axis
P.phi0   = 0;  % initial roll angle
P.theta0 = 0;  % initial pitch angle
P.psi0   = 0;  % initial yaw angle
P.p0     = 0;  % initial body frame roll rate
P.q0     = 0;  % initial body frame pitch rate
P.r0     = 0;  % initial body frame yaw rate

% run trim commands
[x_trim, u_trim]=compute_trim('mavsim_trim',P.Va0,gamma,R);
P.u_trim = u_trim;
P.x_trim = x_trim;

% set initial conditions to trim conditions
% initial conditions
P.pn0    = Xplot2(1,6*i-5);
P.pe0    = Xplot2(1,6*i-4);
P.pd0    = -Xplot2(1,6*i-3);

% P.pn0    = 0;  % initial North position
% P.pe0    = 0;  % initial East position
% P.pd0    = 0;  % initial Down position (negative altitude)

P.u0     = x_trim(4);  % initial velocity along body x-axis
P.v0     = x_trim(5);  % initial velocity along body y-axis
P.w0     = x_trim(6);  % initial velocity along body z-axis
P.phi0   = x_trim(7);  % initial roll angle
P.theta0 = x_trim(8);  % initial pitch angle
P.psi0   = x_trim(9);  % initial yaw angle
P.p0     = x_trim(10);  % initial body frame roll rate
P.q0     = x_trim(11);  % initial body frame pitch rate
P.r0     = x_trim(12);  % initial body frame yaw rate

% compute different transfer functions
[T_phi_delta_a,T_chi_phi,T_theta_delta_e,T_h_theta,T_h_Va,T_Va_delta_t,T_Va_theta,T_v_delta_r]...
    = compute_tf_model(x_trim,u_trim,P);

% linearize the equations of motion around trim conditions
[A_lon, B_lon, A_lat, B_lat] = compute_ss_model('mavsim_trim',x_trim,u_trim);

eig_lon = eig(A_lon);
eig_lat = eig(A_lat);

compute_gains;

% longitudinal state-machine parameters
P.altitude_take_off_zone = 50;
P.altitude_hold_zone = 10;

% sensor parameters(chapter 7)
P.sigma_gyro = 0.13;
P.sigma_accel = 0.0025;
P.beta_abs_pres = 0.125;
P.sigma_abs_pres = 0.01;
P.beta_diff_pres = 0.02;
P.sigma_diff_pres = 0.002;

P.Ts_gps = 1;
P.k_gps = 1/1000;
P.sigma_gps_n = 0.21;
P.sigma_gps_e = 0.21;
P.sigma_gps_altitude = 0.4;
P.sigma_gps_V_g = 0.05;

% filter parameters(chapter 8)
P.alpha_lpf_gyro = 0.1;
P.alpha_lpf_static_pres = 0.1;
P.alpha_lpf_diff_pres = 0.1;

P.bias_gyro_x = 0;             % x-gyro bias
P.bias_gyro_y = 0;             % y-gyro bias
P.bias_gyro_z = 0;             % z-gyro bias

% autopilot guidance model coefficients(chapter 9)
wn_h = 1.5;
zeta_h = 0.707;
P.b_hdot = 2 * zeta_h * wn_h;
P.b_h = wn_h^2;

wn_chi = 0.7;
zeta_chi = 0.707;
P.b_chidot = 2 * zeta_chi * wn_chi;
P.b_chi = wn_chi^2;

P.b_Va = 10;
P.gamma_max = 45*pi/180;

% chapter 10 parameters
P.k_path = 0.05;
P.chi_inf = 40*pi/180;
P.k_orbit = 0.01;

% chapter 11 - path manager
% number of waypoints in data structure
P.size_waypoint_array = 100;
P.R_min = P.Va0^2/P.gravity/tan(P.roll_max);

% create random city map
city_width      = 2000;  % the city is of size (width)x(width)
building_height = 300;   % maximum height of buildings
%building_height = 1;   % maximum height of buildings (for camera)
num_blocks      = 5;    % number of blocks in city
street_width    = .8;   % percent of block that is street.
P.h0 = 100;
P.pd0           = -P.h0;  % initial height of MAV
P.map = createWorld(city_width, building_height, num_blocks, street_width);


function K=kernel_noise(M,dimker)

% Copyright (C) <2007>  <Francesc Moreno-Noguer, Vincent Lepetit, Pascal Fua>
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the version 3 of the GNU General Public License
% as published by the Free Software Foundation.
%
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
% General Public License for more details.
% You should have received a copy of the GNU General Public License
% along with this program. If not, see <http://www.gnu.org/licenses/>.
%
% Francesc Moreno-Noguer, CVLab-EPFL, September 2007.
% fmorenoguer@gmail.com, http://cvlab.epfl.ch/~fmoreno/

MtM=M'*M;
[V,S]=eig(MtM);

K=V(:,dimker:-1:1);


function [x,zo,xs]=estnoisem_noiseseg(yf,noise_seg,pv01,tz,pp)
%ESTNOISEM - estimate noise spectrum using minimum statistics
%
% Usage:    ninc=round(0.016*fs);   % frame increment [fs=sample frequency]
%           ovf=2;                  % overlap factor
%           f=rfft(enframe(s,hanning(ovf*ninc,'periodic'),ninc),ovf*ninc,2);
%           f=f.*conj(f);           % convert to power spectrum
%           x=estnoisem(f,ninc/fs); % estimate the noise power spectrum
%
% Inputs:
%   yf      input power spectra (one row per frame)
%   tz      frame increment in seconds
%           Alternatively, the input state from a previous call (see below)
%   pp      algorithm parameters [optional]
%
% Outputs:
%   x       estimated noise power spectra (one row per frame)
%   zo      output state
%   xs      estimated std error of x (one row per frame)
%           xs seems often to be an underestimate by a factor of 2 or 3
%
% The algorithm parameters are defined in reference [1] from which equation
% numbers are given in parentheses. They are as follows:
%
%        pp.taca      % (11): smoothing time constant for alpha_c [0.0449 seconds]
%        pp.tamax     % (3): max smoothing time constant [0.392 seconds]
%        pp.taminh    % (3): min smoothing time constant (upper limit) [0.0133 seconds]
%        pp.tpfall    % (12): time constant for P to fall [0.064 seconds]
%        pp.tbmax     % (20): max smoothing time constant [0.0717 seconds]
%        pp.qeqmin    % (23): minimum value of Qeq [2]
%        pp.qeqmax    % max value of Qeq per frame [14]
%        pp.av        % (23)+13 lines: fudge factor for bc calculation  [2.12]
%        pp.td        % time to take minimum over [1.536 seconds]
%        pp.nu        % number of subwindows to use [3]
%        pp.qith      % Q-inverse thresholds to select maximum noise slope [0.03 0.05 0.06 Inf ]
%        pp.nsmdb     % corresponding noise slope thresholds in dB/second   [47 31.4 15.7 4.1]
%
% Example use:      y=enframe(s,w,ni);                  % divide speech signal s(n) into
%                                                       % overlapping frames using window w(n)
%                   yf=rfft(y,nf,2);                    % take fourier transform
%                   dp=estnoisem(yf.*conj(yf),tinc);    % estimate the noise
%
% If convenient, you can call estnoisem in chunks of arbitrary size. Thus the following are equivalent:
%
%                   (a) dp=estnoisem(yp(1:300),tinc);
%
%                   (b) [dp(1:100),z]=estnoisem(yp(1:100),tinc);
%                       [dp(101:200),z]=estnoisem(yp(101:200),z);
%                       [dp(201:300),z]=estnoisem(yp(201:300),z);

% This is intended to be a precise implementation of [1] with Table III
% replaced by the updated table 5 from [2]. The only deliberate algorithm
% change is the introduction of a minimum value for 1/Qeq in equation (23).
% This change only affects the first few frames and improves the
% convergence of the algorithm. A minor improveemnt was reported in [3] but
% this has not yet been included.
%
% Refs:
%    [1] Rainer Martin.
%        Noise power spectral density estimation based on optimal smoothing and minimum statistics.
%        IEEE Trans. Speech and Audio Processing, 9(5):504-512, July 2001.
%    [2] Rainer Martin.
%        Bias compensation methods for minimum statistics noise power spectral density estimation
%        Signal Processing, 2006, 86, 1215-1229
%    [3] Dirk Mauler and Rainer Martin
%        Noise power spectral density estimation on highly correlated data
%        Proc IWAENC, 2006

%	   Copyright (C) Mike Brookes 2008
%      Version: $Id: estnoisem.m 1718 2012-03-31 16:40:41Z 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   Modified code, Zheng-Hua Tan, 2012
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nr,nrf]=size(yf);          % number of frames and freq bins

x=zeros(nr,nrf);            % initialize output arrays
xs=zeros(nr,nrf);           % will hold std error in the future
if isempty(yf) && isstruct(tz)             % no real data
    zo=tz;              % just keep the same state
else
    if isstruct(tz)       % take parameters from a previous call
        nrcum=tz.nrcum;
        p=tz.p;          % smoothed power spectrum
        ac=tz.ac;               % correction factor (9)
        sn2=tz.sn2;              % estimated noise power
        pb=tz.pb;               % smoothed noisy speech power (20)
        pb2=tz.pb2;
        pminu=tz.pminu;
        actmin=tz.actmin;   % Running minimum estimate
        actminsub=tz.actminsub;           % sub-window minimum estimate
        subwc=tz.subwc;                   % force a buffer switch on first loop
        actbuf=tz.actbuf;  % buffer to store subwindow minima
        ibuf=tz.ibuf;
        lminflag=tz.lminflag;      % flag to remember local minimum
        tinc=tz.tinc;     % frame increment
        qq=tz.qq;         % parameter structure
    else
        tinc = tz;          % second argument is frame increment
        nrcum=0;            % no frames so far
        % default algorithm constants

        qq.taca=0.0449;    % smoothing time constant for alpha_c = -tinc/log(0.7) in equ (11)
        qq.tamax=0.392;    % max smoothing time constant in (3) = -tinc/log(0.96)
        qq.taminh=0.0133;    % min smoothing time constant (upper limit) in (3) = -tinc/log(0.3)
        qq.tpfall=0.064;   % time constant for P to fall (12)
        qq.tbmax=0.0717;   % max smoothing time constant in (20) = -tinc/log(0.8)
        qq.qeqmin=2;       % minimum value of Qeq (23)
        qq.qeqmax=14;      % max value of Qeq per frame
        qq.av=2.12;             % fudge factor for bc calculation (23 + 13 lines)
        qq.td=1.536;       % time to take minimum over
        qq.nu=8;           % number of subwindows
        qq.qith=[0.03 0.05 0.06 Inf]; % noise slope thresholds in dB/s
        qq.nsmdb=[47 31.4 15.7 4.1];

        if nargin>=4 && ~isempty(pp)
            qqn=fieldnames(qq);
            for i=1:length(qqn)
                if isfield(pp,qqn{i})
                    qq.(qqn{i})=pp.(qqn{i});
                end
            end
        end
    end

    % unpack parameter structure

    taca=qq.taca;    % smoothing time constant for alpha_c = -tinc/log(0.7) in equ (11)
    tamax=qq.tamax;    % max smoothing time constant in (3) = -tinc/log(0.96)
    taminh=qq.taminh;    % min smoothing time constant (upper limit) in (3) = -tinc/log(0.3)
    tpfall=qq.tpfall;   % time constant for P to fall (12)
    tbmax=qq.tbmax;   % max smoothing time constant in (20) = -tinc/log(0.8)
    qeqmin=qq.qeqmin;       % minimum value of Qeq (23)
    qeqmax=qq.qeqmax;      % max value of Qeq per frame
    av=qq.av;             % fudge factor for bc calculation (23 + 13 lines)
    td=qq.td;       % time to take minimum over
    nu=qq.nu;           % number of subwindows
    qith=qq.qith; % noise slope thresholds in dB/s
    nsmdb=qq.nsmdb;   % maximum permitted +ve noise slope in dB/s

    % derived algorithm constants

    aca=exp(-tinc/taca); % smoothing constant for alpha_c in equ (11) = 0.7
    acmax=aca;          % min value of alpha_c = 0.7 in equ (11) also = 0.7
    amax=exp(-tinc/tamax); % max smoothing constant in (3) = 0.96
    aminh=exp(-tinc/taminh); % min smoothing constant (upper limit) in (3) = 0.3
    bmax=exp(-tinc/tbmax); % max smoothing constant in (20) = 0.8
    snrexp = -tinc/tpfall;
    nv=round(td/(tinc*nu));    % length of each subwindow in frames
    if nv<4            % algorithm doesn't work for miniscule frames
        nv=4;
        nu=max(round(td/(tinc*nv)),1);
    end
    nd=nu*nv;           % length of total window in frames
    [md,hd]=mhvals(nd); % calculate the constants M(D) and H(D) from Table III
    [mv,hv]=mhvals(nv); % calculate the constants M(D) and H(D) from Table III
    nsms=10.^(nsmdb*nv*tinc/10);  % [8 4 2 1.2] in paper
    qeqimax=1/qeqmin;  % maximum value of Qeq inverse (23)
    qeqimin=1/qeqmax; % minumum value of Qeq per frame inverse

    if isempty(yf)      % provide dummy initialization
        ac=1;               % correction factor (9)
        subwc=nv;                   % force a buffer switch on first loop
        ibuf=0;
        p=x;          % smoothed power spectrum
        sn2=p;              % estimated noise power
        pb=p;               % smoothed noisy speech power (20)
        pb2=pb.^2;
        pminu=p;
        actmin=repmat(Inf,1,nrf);   % Running minimum estimate
        actminsub=actmin;           % sub-window minimum estimate
        actbuf=repmat(Inf,nu,nrf);  % buffer to store subwindow minima
        lminflag=zeros(1,nrf);      % flag to remember local minimum
    else

        if ~nrcum

            %Initialization excluding 3 left, current (noisy frame), 3 right frames
            By=[]; Cx=[]; dC=[]; Ax=[];
            xNc=find(noise_seg==1); % current noise frame indexs
            L_3f=[ [xNc-3]' [xNc-2]' [xNc-1]' ]; %3 frames(indexes) 'left' of 'xNc'
            R_3f=[ [xNc+1]' [xNc+2]' [xNc+3]' ]; % 3 frames(indexes)  'right' of 'xNc'

            dC=[ L_3f xNc' R_3f ];   SxY=sort(unique(dC(:))); % (frame indexes which should not be considered)

            By=SxY(find(SxY>0)); % (discard 0 or negative frame indexes)
            Ax=1:1:length(noise_seg); Cx = setdiff(Ax,By); % 'By' all unsatisfied frames (ascending sorted order)

            if isempty(Cx) == 0 % satisfied frame indexes
               p=yf(Cx(1),:);   %first satisfied frame from begining (as 'Cx' asced. sorted order)

            else    % otherwise

                Cx=find(noise_seg==0); %speech frames
                if length (Cx) > 3
                   Cx(1:3)=[];  % avoid first 3 speech frames - insense of "only slight better" 'init of p'
                end
                By=[];       % removed 3L-current-3R indexes (else impose to discard all frames in file)- during 'p' update [ ~isempty(intersect(t,By)) ]

                if isempty(Cx) == 1 % not exist 4th speech frame/only contents noisy segment

                      Cx=1;   p=yf(1,:); %no hope (as usual)
                      warning('File seems have only noisy segments ..  noise-power-spectra-estimation may not good!');
                    else
                      p=yf(Cx(1),:); %i.e. 4th speech frame
                end

            end %

            ne_min=p;
            ac=1;               % correction factor (9)
            sn2=p;              % estimated noise power
            pb=p;               % smoothed noisy speech power (20)
            pb2=pb.^2;
            pminu=p;
            actmin=repmat(Inf,1,nrf);   % Running minimum estimate
            actminsub=actmin;           % sub-window minimum estimate
            subwc=nv;                   % force a buffer switch on first loop
            actbuf=repmat(Inf,nu,nrf);  % buffer to store subwindow minima
            ibuf=0;
            lminflag=zeros(1,nrf);      % flag to remember local minimum
        end

        % loop for each frame
        if sum(pv01(1:10))>=1
            p=min(yf(1:min(50,nr),:),[],1);
            ne_min=p; sn2=p; pb=p; pb2=pb.^2; pminu=p;
        end

        for t=Cx(1):nr % start from the "initialization frame for 'p'  %% we use t instead of lambda in the paper
            yft=yf(t,:);        % noise speech power spectrum
            acb=(1+(sum(p)./sum(yft)-1).^2).^(-1);  % alpha_c-bar(t)  (9)
            ac=aca*ac+(1-aca)*max(acb,acmax);       % alpha_c(t)  (10)
            ah=amax*ac.*(1+(p./sn2-1).^2).^(-1);    % alpha_hat: smoothing factor per frequency (11)
            snr=sum(p)/sum(sn2);
            ah=max(ah,min(aminh,snr^snrexp));       % lower limit for alpha_hat (12)

            if noise_seg(t) ||  ~isempty(intersect(t,By)) || (t<11 && sum(pv01(1:10))>=1) %avoid the frames in "By" (3Left, curr. noise,3Right)
            else
                p=ah.*p+(1-ah).*yft;            % smoothed noisy speech power (3)
                b=min(ah.^2,bmax);              % smoothing constant for estimating periodogram variance (22 + 2 lines)
                pb=b.*pb + (1-b).*p;            % smoothed periodogram (20)
                pb2=b.*pb2 + (1-b).*p.^2;     	% smoothed periodogram squared (21)
            end

            qeqi=max(min((pb2-pb.^2)./(2*sn2.^2),qeqimax),qeqimin/(t+nrcum));   % Qeq inverse (23)
            qiav=sum(qeqi)/nrf;             % Average over all frequencies (23+12 lines) (ignore non-duplication of DC and nyquist terms)
            bc=1+av*sqrt(qiav);             % bias correction factor (23+11 lines)
            bmind=1+2*(nd-1)*(1-md)./(qeqi.^(-1)-2*md);      % we use the simplified form (17) instead of (15)
            bminv=1+2*(nv-1)*(1-mv)./(qeqi.^(-1)-2*mv);      % same expression but for sub windows
            kmod=bc*p.*bmind<actmin;        % Frequency mask for new minimum

            if any(kmod)
                actmin(kmod)=bc*p(kmod).*bmind(kmod);
                actminsub(kmod)=bc*p(kmod).*bminv(kmod);
            end
            if subwc>1 && subwc<nv              % middle of buffer - allow a local minimum
                lminflag=lminflag | kmod;    	% potential local minimum frequency bins
                pminu=min(actminsub,pminu);
                sn2=pminu;
            else
                if subwc>=nv                    % end of buffer - do a buffer switch
                    ibuf=1+rem(ibuf,nu);     	% increment actbuf storage pointer
                    actbuf(ibuf,:)=actmin;    	% save sub-window minimum
                    pminu=min(actbuf,[],1);
                    i=find(qiav<qith);
                    nsm=nsms(i(1));          	% noise slope max
                    lmin=lminflag & ~kmod & actminsub<nsm*pminu & actminsub>pminu;
                    if any(lmin)
                        pminu(lmin)=actminsub(lmin);
                        actbuf(:,lmin)=repmat(pminu(lmin),nu,1);
                    end
                    lminflag(:)=0;
                    actmin(:)=Inf;
                    subwc=0;
                end
            end
            subwc=subwc+1;
            x(t,:)=sn2;
            %ne_min=min(ne_min, sn2);
            qisq=sqrt(qeqi);
            % empirical formula for standard error based on Fig 15 of [2]
            xs(t,:)=sn2.*sqrt(0.266*(nd+100*qisq).*qisq/(1+0.005*nd+6/nd)./(0.5*qeqi.^(-1)+nd-1));
        end
    end

    if nargout>1    % we need to store the state for next time
        zo.nrcum=nrcum+nr;      % number of frames so far
        zo.p=p;          % smoothed power spectrum
        zo.ac=ac;               % correction factor (9)
        zo.sn2=sn2;              % estimated noise power
        zo.pb=pb;               % smoothed noisy speech power (20)
        zo.pb2=pb2;
        zo.pminu=pminu;
        zo.actmin=actmin;   % Running minimum estimate
        zo.actminsub=actminsub;           % sub-window minimum estimate
        zo.subwc=subwc;                   % force a buffer switch on first loop
        zo.actbuf=actbuf;  % buffer to store subwindow minima
        zo.ibuf=ibuf;
        zo.lminflag=lminflag;      % flag to remember local minimum
        zo.tinc=tinc;     % must be the last one
        zo.qq=qq;
    end
    if ~nargout
        clf;
        subplot(212);
        plot((1:nr)*tinc,10*log10([sum(yf,2) sum(x,2)]))
        ylabel('Frame Energy (dB)');
        xlabel(sprintf('Time (s)   [%d ms frame incr]',round(tinc*1000)));
        axisenlarge([-1 -1.05]);
        legend('input','noise','Location','Best');
        subplot(211);
        plot(1:nrf,10*log10([sum(yf,1)'/nr sum(x,1)'/nr]))
        ylabel('Power (dB)');
        xlabel('Frequency bin');
        axisenlarge([-1 -1.05]);
        legend('input','noise','Location','Best');
    end
end

function [m,h,d]=mhvals(d)
% Values are taken from Table 5 in [2]
%[2] R. Martin,"Bias compensation methods for minimum statistics noise power
%               spectral density estimation", Signal Processing Vol 86, pp1215-1229, 2006.

% approx: plot(d.^(-0.5),[m 1-d.^(-0.5)],'x-'), plot(d.^0.5,h,'x-')
persistent dmh
if isempty(dmh)
    dmh=[
        1   0       0;
        2   0.26    0.15;
        5   0.48    0.48;
        8   0.58    0.78;
        10  0.61    0.98;
        15  0.668   1.55;
        20  0.705   2;
        30  0.762   2.3;
        40  0.8     2.52;
        60  0.841   3.1;
        80  0.865   3.38;
        120 0.89    4.15;
        140 0.9     4.35;
        160 0.91    4.25;
        180 0.92    3.9;
        220 0.93    4.1;
        260 0.935   4.7;
        300 0.94    5];
end

if nargin>=1
    i=find(d<=dmh(:,1));
    if isempty(i)
        i=size(dmh,1);
        j=i;
    else
        i=i(1);
        j=i-1;
    end
    if d==dmh(i,1)
        m=dmh(i,2);
        h=dmh(i,3);
    else
        qj=sqrt(dmh(i-1,1));    % interpolate using sqrt(d)
        qi=sqrt(dmh(i,1));
        q=sqrt(d);
        h=dmh(i,3)+(q-qi)*(dmh(j,3)-dmh(i,3))/(qj-qi);
        m=dmh(i,2)+(qi*qj/q-qj)*(dmh(j,2)-dmh(i,2))/(qi-qj);
    end
else
    d=dmh(:,1);
    m=dmh(:,2);
    h=dmh(:,3);
end


function [oa, pa, K, CM] = USFE_MSTV(HSI, Tr, Te, dim, Trees)
[FE_MSTV]= MSTV_Xu(HSI, dim);
[acc_Mean,acc_std,CM]=RF_ntimes_overal(FE_MSTV,Tr,Te,Trees);
pa=acc_Mean(1:dim,1);
oa=acc_Mean(dim+2,1);
K=acc_Mean(dim+3,1);


% objdall1n - objective function of DAL with non-negative L1 regularization
%
% Copyright(c) 2009-2011 Ryota Tomioka
%                   2011 Shigeyuki Oba
% This software is distributed under the MIT license. See license.txt

function varargout=objdall1n(aa, info, prob, ww, uu, A, B, lambda, eta)

m = length(aa);
n = length(ww);

if isempty(info.ATaa)
  info.ATaa=A.Ttimes(aa);
end

vv = ww+eta(1)*info.ATaa;

if nargout<=3
  [floss, gloss, hmin]=prob.floss.d(aa, prob.floss.args{:});
else
  [floss, gloss, hloss, hmin]=prob.floss.d(aa, prob.floss.args{:});
end

[vsth,ss] = l1n_softth(vv,eta(1)*lambda);
info.wnew = vsth;
info.spec = ss;

fval = floss+0.5*sum(vsth.^2)/eta(1);
if ~isempty(uu)
  u1   = uu+eta(2)*(B'*aa);
  fval = fval + 0.5*sum(u1.^2)/eta(2);
end

varargout{1}=fval;

if nargout<=2
  varargout{2} = info;
else
  gg  = gloss+A.times(vsth);
  soc = sum(((vsth-ww)/eta(1)).^2);
  if ~isempty(uu)
    gg  = gg+B*u1;
    soc = soc+sum((B'*aa).^2);
  end

  if soc>0
    info.ginfo = norm(gg)/(sqrt(min(eta)*hmin*soc));
  else
    info.ginfo = inf;
  end
  varargout{2} = gg;

  if nargout==3
    varargout{3} = info;
  else
    I = find(vv>eta(1)*lambda);
    len = length(I);
    AF = A.slice(I);

    switch(info.solver)
     case 'cg'
      prec=hloss+spdiag(eta(1)*sum(AF.^2,2));
      if ~isempty(uu)
        prec =prec+spdiag(eta(2)*sum(B.^2,2));
      end
      varargout{3} = struct('hloss',hloss,'AF',AF,'I',I,'n',n,'prec',prec,'B',B);
     otherwise
      if length(I)>0
        varargout{3} = hloss+eta(1)*AF*AF';
      else
        varargout{3} = hloss;
      end
      if ~isempty(uu)
        varargout{3} = varargout{3}+eta(2)*B*B';
      end
    end
    varargout{4} = info;
  end
end


%QUAD2TRI Convert quadrilaterals to triangles.
%
%   [ SOUT ] = QUAD2TRI( SIN, IORDER ) Convert quadrilateral to triangular cells.
%
%       Input       Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       sin         struct                 Grid or problem struct with
%                                          p, c (and optionally s and b) fields
%       iorder      1/2/{0}                Node ordering of triangles.
%                                            0 : Automatically select best splitting
%                                            1 : 1-2-3 / 1-3-4 splitting
%                                            2 : 2-3-4 / 1-2-4 splitting
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       sout        struct                 Output grid or problem struct
%
%   See also HEX2TET, TET2HEX, TRI2QUAD

% Copyright 2013-2022 Precise Simulation, Ltd.


function [pdag, G] = learn_struct_pdag_pc(cond_indep, n, k, varargin)
% LEARN_STRUCT_PDAG_PC Learn a partially oriented DAG (pattern) using the PC algorithm
% P = learn_struct_pdag_pc(cond_indep, n, k, ...)
%
% n is the number of nodes.
% k is an optional upper bound on the fan-in (default: n)
% cond_indep is a boolean function that will be called as follows:
%   feval(cond_indep, x, y, S, ...)
% where x and y are nodes, and S is a set of nodes (positive integers),
% and ... are any optional parameters passed to this function.
%
% The output P is an adjacency matrix, in which
% P(i,j) = -1 if there is an i->j edge.
% P(i,j) = P(j,i) = 1 if there is an undirected edge i <-> j
%
% The PC algorithm does structure learning assuming all variables are observed.
% See Spirtes, Glymour and Scheines, "Causation, Prediction and Search", 1993, p117.
% This algorithm may take O(n^k) time if there are n variables and k is the max fan-in,
% but this is quicker than the Verma-Pearl IC algorithm, which is always O(n^n).

sep = cell(n,n);
ord = 0;
done = 0;
G = ones(n,n);
G=setdiag(G,0);
while ~done
  done = 1;
  [X,Y] = find(G);
  for i=1:length(X)
    x = X(i); y = Y(i);
    %nbrs = mysetdiff(myunion(neighbors(G, x), neighbors(G,y)), [x y]);
    nbrs = mysetdiff(neighbors(G, y), x);  % bug fix by Raanan Yehezkel <raanany@ee.bgu.ac.il> 6/27/04
    nbrs = unique(nbrs); % bug fix Immer Ebert ebert@tree.com 3/278/11
    if length(nbrs) >= ord & G(x,y) ~= 0
      done = 0;
      %SS = subsets(nbrs, ord, ord); % all subsets of size ord
      SS = subsets1(nbrs, ord);
      for si=1:length(SS)
	S = SS{si};
	if feval(cond_indep, x, y, S, varargin{:})
	  %if isempty(S)
 	  %  fprintf('%d indep of %d ', x, y);
	  %else
 	  %  fprintf('%d indep of %d given ', x, y); fprintf('%d ', S);
 	  %end
 	  %fprintf('\n');

	  % diagnostic
	  %[CI, r] = cond_indep_fisher_z(x, y, S, varargin{:});
	  %fprintf(': r = %6.4f\n', r);

	  G(x,y) = 0;
	  G(y,x) = 0;
	  sep{x,y} = myunion(sep{x,y}, S);
	  sep{y,x} = myunion(sep{y,x}, S);
	  break; % no need to check any more subsets
	end
      end
    end
  end
  ord = ord + 1;
end

% Create the minimal pattern,
% i.e., the only directed edges are V structures.
pdag = G;
[X, Y] = find(G);
% We want to generate all unique triples x,y,z
% This code generates x,y,z and z,y,x.
for i=1:length(X)
  x = X(i);
  y = Y(i);
  Z = find(G(y,:));
  Z = mysetdiff(Z, x);
  for z=Z(:)'
    if G(x,z)==0 & ~ismember(y, sep{x,z}) & ~ismember(y, sep{z,x})
      %fprintf('%d -> %d <- %d\n', x, y, z);
      pdag(x,y) = -1; pdag(y,x) = 0;
      pdag(z,y) = -1; pdag(y,z) = 0;
    end
  end
end

% Convert the minimal pattern to a complete one,
% i.e., every directed edge in P is compelled
% (must be directed in all Markov equivalent models),
% and every undirected edge in P is reversible.
% We use the rules of Pearl (2000) p51 (derived in Meek (1995))

old_pdag = zeros(n);
iter = 0;
while ~isequal(pdag, old_pdag)
  iter = iter + 1;
  old_pdag = pdag;
  % rule 1
  [A,B] = find(pdag==-1); % a -> b
  for i=1:length(A)
    a = A(i); b = B(i);
    C = find(pdag(b,:)==1 & G(a,:)==0); % all nodes adj to b but not a
    if ~isempty(C)
      pdag(b,C) = -1; pdag(C,b) = 0;
      %fprintf('rule 1: a=%d->b=%d and b=%d-c=%d implies %d->%d\n', a, b, b, C, b, C);
    end
  end
  % rule 2
  [A,B] = find(pdag==1); % unoriented a-b edge
  for i=1:length(A)
    a = A(i); b = B(i);
    if any( (pdag(a,:)==-1) & (pdag(:,b)==-1)' );
      pdag(a,b) = -1; pdag(b,a) = 0;
      %fprintf('rule 2: %d -> %d\n', a, b);
    end
  end
  % rule 3
  [A,B] = find(pdag==1); % a-b
  for i=1:length(A)
    a = A(i); b = B(i);
    C = find( (pdag(a,:)==1) & (pdag(:,b)==-1)' );
    % C contains nodes c s.t. a-c->ba
    G2 = setdiag(G(C, C), 1);
    if any(G2(:)==0) % there are 2 different non adjacent elements of C
      pdag(a,b) = -1; pdag(b,a) = 0;
      %fprintf('rule 3: %d -> %d\n', a, b);
    end
  end
end


%GRIDMERGE Merge two grids.
%
%   [ GRID ] = GRIDMERGE( GRID1, IND1, GRID2, IND2, I_DELETE, I_CHECK ) Merges
%   the matching boundaries edges and faces specified by boundary numbers IND1
%   in GRID1 and IND2 in GRID2. The algorithm tries to match up the boundary
%   with the fewest boundary edges/faces and nodes to the closest corresponding
%   ones on GRID2. The optional argument I_DELETE selects which grid to delete
%   duplicate nodes from (I_DELETE is 2 per default). I_CHECK toggles error
%   checking on/off.
%
%   Can optionally be called as GRIDMERGE( GRID1, GRID2 ) where IND1 and IND2
%   are determined by any shared boundary numbers (also in this case boundaries
%   and subdomains are not renumbered).
%
%   Examples:
%
%      1) Create a 2D rivet by joining a rectangle and half circle.
%
%      grid1 = rectgrid( 4, 10, [0.3 0.7;0 1] );
%      grid2 = circgrid( 4, 3, 0.5, [0.5;1] );
%      grid2 = delcells( grid2, 'y<=1' );
%      grid  = gridmerge( grid1, 3, grid2, 4 );
%
%      2) Create a flow over cylinder benchmark grid by merging three grids.
%
%      grid1 = ringgrid( [0.05 0.06 0.08 0.11 0.15], 32, [], [], [0.2;0.2] );
%      grid2 = holegrid( 8, 1, [0 0.41;0 0.41], 0.15, [0.2;0.2] );
%      grid2 = gridmerge( grid1, 5:8, grid2, 1:4 );
%      grid3 = rectgrid( [0.41 0.5 0.7 1 1.4 1.8 2.2], 8, [0.41 2.2;0 0.41] );
%      grid  = gridmerge( grid3, 4, grid2, 6 );
%
%      3) Create a 3D grid with two brackets attached to an I-beam section.
%
%      grid01 = ringgrid( 1, 20, 0.03, 0.06, [0;0] );
%      indc01 = selcells( grid01, 'y<=sqrt(eps)' );
%      grid01 = delcells( grid01, indc01 );
%
%      grid02 = holegrid( 5, 1, .06*[-1 1;-1 1], .03, [0;0] );
%      indc02 = selcells( grid02, 'y>=-sqrt(eps)' );
%      grid02 = delcells( grid02, indc02 );
%      grid2d = gridmerge( grid01, [5 6], grid02, [7 8] );
%
%      grid1 = gridextrude( grid2d, 1, 0.02 );
%      grid1 = gridrotate( grid1, pi/2, 1 );
%      grid2 = grid1;
%      grid1.p(2,:) = grid1.p(2,:) + 0.03;
%      grid2.p(2,:) = grid2.p(2,:) - 0.01;
%
%      x_coord = [ -0.08 linspace(-0.06,0.06,6) 0.08 ];
%      y_coord = [ -0.2 -0.15 -0.1 -0.05 -0.03 -0.01 ...
%                   0.01  0.03  0.05  0.1  0.15  0.2 ];
%      grid3 = blockgrid( x_coord, y_coord, 1, ...
%                         [-0.08 0.08;-0.2 0.2;-0.08 -0.06] );
%      grid4 = blockgrid( 1, y_coord, 5, ...
%                         [-0.01 0.01;-0.2 0.2;-0.18 -0.08] );
%      grid5 = grid3;
%      grid5.p(3,:) = grid5.p(3,:) - 0.12;
%
%      grid = gridmerge( grid1, 8, grid3, 6 );
%      grid = gridmerge( grid2, 8, grid, 19 );
%      grid = gridmerge( grid4, 6, grid, 24, 1 );
%      grid = gridmerge( grid5, 6, grid, 33, 2 );
%
%   See also GRIDEXTRUDE, GRIDREVOLVE, GRIDROTATE, GRIDSCALE

% Copyright 2013-2022 Precise Simulation, Ltd.


%ESTIMATEAFFINE3D  Computes an optimal affine transformation between two 3D point sets
%
%     M = cv.estimateAffine3D(src, dst)
%     [M, inliers] = cv.estimateAffine3D(...)
%     [...] = cv.estimateAffine3D(..., 'OptionName', optionValue, ...)
%
% ## Input
% * __src__ First input 3D point set containing `(X,Y,Z)`. Cell array of
%   3-element vectors `{[x,y,z],...}` or Nx3/Nx1x3/1xNx3 numeric array.
% * __dst__ Second input 3D point set containing `(x,y,z)`. Same size and type
%   as `src`.
%
% ## Output
% * __M__ Output 3D affine transformation matrix 3x4 of the form
%   `[a11 a12 a13 b1; a21 a22 a23 b2; a31 a32 a33 b3]`
% * __inliers__ Output vector of same length as number of points, indicating
%   which points are inliers (1-inlier, 0-outlier).
% * __result__ success flag.
%
% ## Options
% * __RansacThreshold__ Maximum reprojection error in the RANSAC algorithm to
%   consider a point as an inlier. default 3.0.
% * __Confidence__ Confidence level, between 0 and 1, for the estimated
%   transformation. Anything between 0.95 and 0.99 is usually good enough.
%   Values too close to 1 can slow down the estimation significantly. Values
%   lower than 0.8-0.9 can result in an incorrectly estimated transformation.
%   default 0.99.
%
% It computes:
%
%     [x; y; z] = [a11 a12 a13; a21 a22 a23; a31 a32 a33] * [X; Y; Z] + [b1; b2; b3]
%
% The function estimates an optimal 3D affine transformation between two 3D
% point sets using the RANSAC algorithm.
%
% See also: cv.estimateAffine2D
%


classdef TestMorphologyEx
    %TestMorphologyEx

    properties (Constant)
        img = uint8([...
            0 0 0 0 0 0 0 0 0 0;...
            0 0 0 0 0 0 0 0 0 0;...
            0 0 0 0 0 0 0 0 0 0;...
            0 0 0 1 1 1 0 0 0 0;...
            0 0 0 1 1 1 0 0 0 0;...
            0 0 0 1 1 1 0 0 0 0;...
            0 0 0 0 0 0 0 0 0 0;...
            0 0 0 0 0 0 0 0 0 0;...
            0 0 0 0 0 0 0 0 0 0;...
            0 0 0 0 0 0 0 0 0 0;...
        ]);
    end

    methods (Static)
        function test_1
            result = cv.morphologyEx(TestMorphologyEx.img, 'Tophat');
            validateattributes(result, {class(TestMorphologyEx.img)}, ...
                {'size',size(TestMorphologyEx.img)});
        end

        function test_2
            result = cv.morphologyEx(TestMorphologyEx.img, 'Tophat', ...
                'Element',eye(3), 'Anchor',[1,1], 'Iterations',1, ...
                'BorderType','Constant', 'BorderValue',255);
            validateattributes(result, {class(TestMorphologyEx.img)}, ...
                {'size',size(TestMorphologyEx.img)});
        end

        function test_error_argnum
            try
                cv.morphologyEx();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end


function test_boundary()

grid = logical(randi(2,100,100)-1);

for j = 1:20
  ndx = randi(100^2);

  filled = imfill(~grid, ndx, 8);
  component = ~(xor(filled,grid));
  t0 = tic();
  boundary = imdilate(component, strel('square', 3)) - component;
  t_dil = toc(t0);
  fprintf('imdilate: %f,   ', t_dil);
  t0 = tic();
  fast_boundary = iris.terrain_grid.find_boundary(component);
  t_bound = toc(t0);
  fprintf('fast: %f,   ratio: %f\n', t_bound, t_dil/t_bound);

  figure(23)
  subplot 211
  imshow(boundary, 'InitialMagnification', 'fit');
  subplot 212
  imshow(fast_boundary, 'InitialMagnification', 'fit');
  assert(all(all(boundary == fast_boundary)));
end


function [ALLEEG cfg] = pop_est_validateMVAR(ALLEEG,typeproc,varargin)
%
% Validate a fitted VAR model. With two inputs, this function generates a
% GUI where the validation scheme can be specified. Validation consists of
% statistical tests for "whiteness" of fitted VAR model residuals [1,2],
% consistency of the fitted model [1,3] and stability of fitted model [1-3]
%
% Input:
%
%   ALLEEG:     Array of EEGLAB data structures containing fitted MODEL
%   typeproc:   reserved for future use. Use 0
%
% Optional:
%
%     'whitenessCriteria':    Cell array containing names of residual whiteness
%                             test criteria to evaluate. See [1, 2] for details.
%                             Possible Values: {'Ljung-Box','ACF','Box-Pierce','Li-McLeod'}
%                             Default Value  : all
%                             Data Input Type: cell array
%
%
%     'checkWhiteness':       Whether or not to check whiteness.
%                             Default Value  : true
%                             Data Input Type: boolean
%
%     'checkConsistency':     Whether or not to check consistency. See [1,3]
%                             for details.
%                             Default Value  : true
%                             Data Input Type: boolean
%
%     'checkStability':       Whether or not to check stability. See
%                             [1-3] for details.
%                             Default Value  : true
%                             Data Input Type: boolean
%
%     'alpha':                significance level for determining whiteness
%                             Data Input Range: [0 1]
%                             Default Value   : 0.05
%                             Data Input Type : real number (double)
%
%     'prctWinToSample':      percent of time windows to randomly select
%                             Data Input Range: [0 100]
%                             Default Value   : 100
%                             Data Input Type : real number (double)
%
%     'verb':                 verbosity level (0=no output, 1=text, 2=gui)
%
%
% Output:
%
%     whitestats:             Structure containing whiteness statistics.
%                             See est_checkMVARWhiteness() for details on
%                             structure format
%
%     PC:                     Vector of percent consistency estimates for
%                             each window.
%
%     stability:              Vector of stability estimates for each window
%     cfg:                    parameter configuration object
%
% See Also: est_checkMVARWhiteness(), est_checkMVARStability(),
%           est_checkMVARConsistency, pop_est_fitMVAR()
%
%
% References:
%
% [1] Mullen T (2010) The Source Information Flow Toolbox (SIFT):
%   Theoretical Handbook and User Manual. Chapter 3.6 and 6.
%   Available at: http://www.sccn.ucsd.edu/wiki/Sift
%
% [2] Lutkepohl, H. (2007) New Introduction to Time Series Analysis.
%   Springer.
%
% [3] Ding M, Bressler SL, Yang W, Liang H (2000) Short-window spectral
%   analysis of cortical event-related potentials by adaptive multivariate
%   autoregressive modeling: data preprocessing, model validation, and
%   variability assessment. Biol. Cybern. 83:35-45
%
% Author: Tim Mullen, 2010, SCCN/INC, UCSD.
% Email:  tim@sccn.ucsd.edu

% This function is part of the Source Information Flow Toolbox (SIFT)
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

if nargin<2
    typeproc = 0;
end

fcnName     = strrep(mfilename,'pop_','');
fcnHandle   = str2func(fcnName);

% check the dataset
res = hlp_checkeegset(ALLEEG,{'model'});
if ~isempty(res)
    error(['SIFT:' fcnName],res{1});
end

if isfield(ALLEEG(1).CAT.configs,fcnName)
    % get default configuration (from prior use) and merge with varargin
    varargin = [hlp_struct2varargin(ALLEEG(1).CAT.configs.(fcnName)) varargin];
end

if strcmpi(typeproc,'nogui')
    % get the config from function
    cfg = arg_tovals(arg_report('rich',fcnHandle,[{'EEG',ALLEEG(1)},varargin]),false);
else
    % render the GUI
    [PGh figh] = feval(['gui_' fcnName],ALLEEG(1),varargin{:});

    if isempty(PGh)
        % user chose to cancel
        cfg = [];
        return;
    end

    % get the specification of the PropertyGrid
    ps = PGh.GetPropertySpecification;
    cfg = arg_tovals(ps,false);
end

drawnow;

if strcmpi(typeproc,'cfg_only')
    return;
end

% Apply model validation routines
for cnd=1:length(ALLEEG)

    [ALLEEG(cnd).CAT.VALIDATION.whitestats ...
     ALLEEG(cnd).CAT.VALIDATION.PCstats    ...
     ALLEEG(cnd).CAT.VALIDATION.stabilitystats ...
     ALLEEG(cnd).CAT.VALIDATION.residualstats] ...
        = est_validateMVAR('EEG',ALLEEG(cnd),cfg);

    if ~isempty(cfg)
        % store the configuration structure
        ALLEEG(cnd).CAT.configs.(fcnName) = cfg;
    end
end


function [ id, type, rep, field, symm, nrow, ncol, nnz, indx, jndx, value ] ...
  = mm_file_read ( input_unit )

%*****************************************************************************80
%
%% MM_FILE_READ reads data from a Matrix Market file.
%
%  Discussion:
%
%    The data may be either sparse coordinate format, or dense array format.
%
%    The unit input_unit must be open, and the file will be rewound on return.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer INPUT_UNIT, the input unit identifier.
%
%    Output, character ( len = 14 ) ID, the Matrix Market identifier.
%    This value must be '%%MatrixMarket'.
%
%    Output, character ( len = 6 ) TYPE, the Matrix Market type.
%    This value must be 'matrix'.
%
%    Output, character ( len = 10 ) REP, the Matrix Market 'representation'
%    indicator.  Possible values include:
%    'coordinate'   (for sparse data)
%    'array'        (for dense data)
%    'elemental'    (to be added)
%
%    Output, character ( len = 7 ) FIELD, the Matrix Market 'field'.
%    Possible values include:
%    'real'
%    'double'
%    'complex'
%    'integer'
%    'pattern'
%
%    Output, character ( len = 19 ) SYMM, the Matrix Market symmetry.
%    Possible values include:
%    'symmetric'
%    'hermitian'
%    'skew-symmetric'
%    'general'
%
%    Output, integer NROW, the number of rows in the matrix.
%
%    Output, integer NCOL, the number of columns in the matrix.
%
%    Output, integer NNZ, the number of nonzero entries required to store
%    the matrix, if REP = 'coordinate'.
%
%    Output, integer INDX(NNZ), the row indices for coordinate format.
%    Not used if REP is 'array'.
%
%    Output, integer JNDX(NNZ), the column indices for coordinate format.
%    Not used if REP is 'array'.
%
%    Output, complex VALUE(NNZ), the matrix values.
%

%
%  Read and check the header line.
%
  [ id, type, rep, field, symm ] = mm_header_read ( input_unit );

  ok = mm_header_check ( id, type, rep, field, symm );

  if ( ~ok )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MM_FILE_READ - Fatal error!\n' );
    fprintf ( 1, '  The header information was not acceptable.\n' );
    error ( 'MM_FILE_READ - Fatal error!' );
  end
%
%  Read through the comment lines:
%
  while ( 1 )

    comment = mm_comment_read ( input_unit );

    if ( comment(1) ~= '%' )
      break;
    end

  end
%
%  The line we just read is not a comment.  It is presumably the SIZE line.
%
  [ nrow, ncol, nnz ] = mm_size_read_string ( comment, rep, symm );
%
%  Read the data values.
%
  [ value, indx, jndx ] = mm_values_read ( input_unit, rep, field, nnz );

  return
end


function pout = polydern(p,diffvar)
% polydern: partial derivatives of a n-dimensional polynomial model created by polyfitn.
% usage: pout = polydern(p,diffvar)
%
% arguments: (input)
%  p - a structure containing a polynomial regression model.  See polyfitn
%      for details.
%
%  diffvar - integer, which variable to compute the derivative with respect
%      to. If diffvar is a character string, then it refers to an explicit
%      variable in the model that is found in the VarNames field. If so,
%      the variable name must be an exact match for one of the variables
%      listed in p.VarNames.
%
% arguments: (output)
% pout - a structure containing a polynomial regression model which
%           describes the partial derivative with respect to variable #i.
%
% Note:
% Polydern modifies the "ModelTerms" and "Coefficients" fields in creating
% the output polynomial. ParameterVar, parameterStd are also updated to
% have the proper shape, as well as scaling them to reflect the derivative.
%
% The other fields (R2, RMSE, VarNames) are copied without change, even
% though the error measures are no longer meaningful for the derivative
% polynomial.
%
% This function uses polynomial structure variables created by polyfitn,
% which is part of the PolyfitnTools package by John D'Errico:
%     http://www.mathworks.com/matlabcentral/fileexchange/10065
%
% See also: polyder, polyfitn, polyvaln
%
% Polydern author: Jason Goodman
% Error checks, ParameterStd/Var updates and allowance for the variable
% name itself instead of an index added by John D'Errico

% check the number of input args
if nargin ~= 2
  error('POLYDERN:argumentcount','Exactly two arguments are required')
end

% is p a struct, created by polyfitn?
if ~isstruct(p) || ~isfield(p,'ModelTerms') || ~isfield(p,'Coefficients')
  error('POLYDERN:invalidp','p must be a struct as created by polyfitn')
end

% check diffvar too. scalar, numeric, integer, real, in the proper range,
% or it must be character, an exact match for one of the variables listed
% in p.VarNames.
if ischar(diffvar)
  ind = ismember(p.VarNames,diffvar);
  if sum(ind) == 1
    % we have a hit
    diffvar = find(ind);
  else
    error('POLYDERN:invaliddiffvar','diffvar must be a scalar numeric real integer value, or a valid variable name')
  end
elseif ~isnumeric(diffvar) || (numel(diffvar) ~= 1) || (round(diffvar) ~= diffvar) || ~isreal(diffvar)
  % diffvar must be scalar, numeric, etc. Also verify that it is real to
  % be complete
  error('POLYDERN:invaliddiffvar','diffvar must be a scalar numeric real integer value, or a valid variable name')
elseif (diffvar < 1) || (diffvar > size(p.ModelTerms,2))
  % test that diffvar is at least 1, and is not larger than the
  % number of variables in the nmodel
  error('POLYDERN:invaliddiffvar', ...
    ['diffvar (=',num2str(diffvar), ...
    ') cannot represent the variable index to be differentiated, as it is out of range for this model'])
end

pout = p;
pout.ModelTerms = [];
pout.Coefficients = [];

remainingterms = true(1,size(p.ModelTerms,1));
jout = 1;
for jin=1:length(p.Coefficients)
    if (p.ModelTerms(jin,diffvar) ~= 0)
        pout.Coefficients(jout) = p.Coefficients(jin).*p.ModelTerms(jin,diffvar);
        pout.ModelTerms(jout,:) = p.ModelTerms(jin,:);
        pout.ModelTerms(jout,diffvar) = pout.ModelTerms(jout,diffvar)-1;
        jout = jout + 1;
    else
        % this term got dropped from the model
        remainingterms(jin) = false;
    end
end

if (jout == 1)   % Polynomial has no terms in this variable: deriv is zero
    [ncoeff, nvars] = size(p.ModelTerms);
    pout.ModelTerms(1,:) = zeros(1,nvars);
    pout.Coefficients = 0;

    pout.ParameterVar = 0;
    pout.ParameterStd = 0;
else
    % at least some terms remain, so update the parameter variances and
    % standard deviations. First, drop those terms that died.
    pout.ParameterVar = reshape(p.ParameterVar(remainingterms),1,[]);
    pout.ParameterStd = reshape(p.ParameterStd(remainingterms),1,[]);

    % scale them appropriately. Thus if std(A) is sigma, then std(k*A)
    % is k*sigma, but multiply the corresponding variances by k.^2.
    scale = p.ModelTerms(remainingterms,diffvar).';
    pout.ParameterStd = pout.ParameterStd.*scale;
    pout.ParameterVar = pout.ParameterVar.*scale.^2;
end


%IKINE_SYM  Symbolic inverse kinematics
%
% Q = R.IKINE_SYM(K, OPTIONS) is a cell array (Cx1) of inverse kinematic
% solutions of the SerialLink object ROBOT.  The cells of Q represent the
% solutions for each joint, ie. Q{1} is the solution for joint 1.  A
% cell may contain an array of solutions. The solution is expressed in terms
% of other joint angles and elements of the desired end-point pose which is
% represented by the symbolic matrix (3x4) with elements
%      nx ox ax tx
%      ny oy ay ty
%      nz oz az tz
% where the first three columns specify orientation and the last column
% specifies translation.
%
% K <= N is the number of joint angles solved for.
%
% Options::
%
% 'file',F    Write the solution to an m-file named F
% 'Tpost',T   Add a symbolic 4x4 matrix T to the end of the chain
%
% Example::
%
%         mdl_planar2
%         sol = p2.ikine_sym(2);
%         length(sol)
%
%         q1 = sol{1}   % are the solution for joint 1
%         q2 = sol{2}   % is the solution for joint 2
%         length(q1)
%         ans =
%               2     % there are 2 solutions for this joint
%         q1(1)       % one solution for q1
%         q1(2);      % the other solution for q1
%
% Notes::
% - ignores tool and base transforms.
%
% References::
% - Robot manipulators: mathematics, programming and control
%   Richard Paul, MIT Press, 1981.
% - The kinematics of manipulators under computer control,
%   D.L. Pieper, Stanford report AI 72, October 1968.
%
% Notes::
% - Requires the MATLAB Symbolic Math Toolbox.
% - This code is experimental and has a lot of diagnostic prints.
% - Based on the classical approach using Pieper's method.

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function out = ikine_sym(robot, N, varargin)

    %
    % Given a robot model the following steps are performed:
    % 1. Convert model to symbolic form
    % 2. Find relevant trig equations and solve them for joint angles
    % 3. Write an M-file to implement the solution
    %      xikine(T)
    %      xikine(T, S) where S is a 3 vector with elements 1 or 2 to select
    %       the first or second solution for the corresponding joint.
    %
    % TODO:
    %  - handle the wrist joints, only first 3 joints so far
    %  - handle base and tool transforms
    %  - allow 3DOF solution which is rotation, ie. wrist

    opt.file = [];
    opt.Tpost = [];
    opt.all = false;
    opt = tb_optparse(opt, varargin);

    % make a symbolic representation of the passed robot
    srobot = SerialLink(robot);  % make a deep copy
    srobot = sym(srobot);  % convert to symbolic
    q = srobot.gencoords();

    % define symbolic elements of the homogeneous transform
    syms nx ox ax tx real
    syms ny oy ay ty real
    syms nz oz az tz real
    syms d4 real

    % inits
    Q = {};
    trigsubOld = [];
    trigsubNew = [];

    % loop over each joint variable
    for j=1:N
        fprintf('----- solving for joint %d\n', j);

        % create some equations to sift through
        [left,right] = pieper(srobot, j, 'left', opt.Tpost);

        % decide which equations to look at
        if j <= 3
            % for first three joints only focus on translational part
            left = left(1:3, 4); left = left(:);
            right = right(1:3, 4); right = right(:);
        else
            % for last three joints only focus on rotational part
            left = left(1:3, 1:3); left = left(:);
            right = right(1:3, 1:3); right = right(:);
        end

        % substitute sin/cos for preceding joint as Sj/Cj, essentially removes
        % the joint variables from the equations and treats them as constants.
        if ~isempty(trigsubOld)
            left = rsubs(left, trigsubOld, trigsubNew);
            right = rsubs(right, trigsubOld, trigsubNew);
        end

        % then simplify the LHS
        %   do it after the substitution to prevent sum of angle terms being introduced
        left = simplify(left);

        % search for a solveable equation:
        %    function of current joint variable on the LHS
        %    constant element on the RHS
        k = NaN;
        for i=1:length(left)
            fprintf('%d: %s\n', i, char(left(i) == right(i)));
            if hasonly(left(i), j) && isconstant(right(i))
                k = i;
            end
        end

        eq = [];

        if ~isnan(k)
            % create the equation to solve: LHS-RHS == 0
            fprintf('choosing equation %d\n', k);

            eq = left(k) - right(k);
        else
            % ok, we weren't lucky, try another strategy

            % find all equations:
            %    function of current joint variable on the LHS

            k = [];
            for i=1:length(left)
                % has qj on the left
                if hasonly(left(i), j)
                    k = [k i];
                end
            end

            % hopefully we found at least two of them
            if length(k) < 2
                continue;
            end

            % we did, lets see if the sum square RHS is constant
            for kk = nchoosek(k, 2)'
                fprintf('No simple equation, let''s square and add RHS %d %d\n', kk);
                rhs = simplify(right(kk(1))^2 + right(kk(2))^2); % was simple
                if isconstant( rhs )
                    eq = simplify( expand( left(kk(1))^2 + left(kk(2))^2 ) ) - rhs;
                    break
                end
            end
            if isempty(eq)
                fprintf('** can''t solve this equation, out of options');
                k
                left(k)==right(k)
                error('can''t solve this equation');
            end
            % ensure that S^2+C^2=1 substitutions are made
            eq = subs(eq, trigsubOld, trigsubNew);
            disp(eq)
        end

        % expand the list of joint variable subsitutions
        fprintf('subs sin/cos q%d for S/C\n', j);
        trigsubOld = [trigsubOld mvar('sin(q%d)', j)^2+mvar('cos(q%d)', j)^2 mvar('sin(q%d)', j) mvar('cos(q%d)', j) ];
        trigsubNew = [trigsubNew 1 mvar('S%d', j) mvar('C%d', j) ];

        % now solve the equation
        if srobot.links(j).isrevolute()
            % for revolute joint it will be a trig equation, do we know how to solve it?
            Q{j} = solve_joint(eq, j );
            if isempty(Q)
                warning('RTB:ikine_sym', 'can''t solve this kind of equation');
            end
        else
            fprintf('prismatic case\n')
            q = mvar('q%d', j);
            Q{j} = solve( eq == 0, q);
        end
    end

    % final simplification
    %  get rid of C^2+S^2 and C^4, S^4 terms
    fprintf('**final simplification pass\n')

    trigsubOld = [];
    trigsubNew = [];
    for j=1:N
        trigsubOld = [trigsubOld mvar('S%d', j) mvar('C%d', j) ];
        trigsubNew = [trigsubNew mvar('sin(q%d)', j) mvar('cos(q%d)', j) ];
    end

    Q = simplify_powers(Q, N, trigsubOld, trigsubNew);

    % Q is a cell array of equations for joint variables
    if nargout > 0
        out = Q;
    end

    if ~isempty(opt.file)
        fprintf('**generate MATLAB code\n')
        gencode(Q);
    end
end

%PIEPER Return a set of equations using Pieper's method
%
% [L,R] = pieper(robot, n, which)
%
% If robot has link matrix A1 A2 A3 A4 then returns 12 equations from equating the coefficients of
%
%  A1' T = A2 A3 A4     n=1, which='left'
%  A2' A1' T = A3 A4    n=2, which='left'
%  A3' A2' A1' T = A4   n=3, which='left'
%
%  T A4' = A1 A2 A3     n=1, which='right'
%  T A4' A3' = A1 A2    n=2, which='right'
%  T A4' A3' A2' = A1   n=3, which='right'
%
% A' denotes inversion not transposition
%
% Judicious choice of the equations can lead to joint solutions

function [L,R] = pieper(robot, n, which, Tpost)

    if nargin < 3
        which = 'left';
    end
    if nargin < 4 || isempty(Tpost)
        Tpost = transl(zeros(robot.n, 3));
    end
    assert(n <= robot.n, 'RTB:ikine_sym:badarg', 'N is greater than number of joints');

    syms nx ox ax tx real
    syms ny oy ay ty real
    syms nz oz az tz real

    T = [nx ox ax tx
        ny oy ay ty
        nx oz az tz
        0  0  0  1 ];

    T = inv(robot.base.T) * T * inv(robot.tool.T);

    q = robot.gencoords();

    % Create the symbolic A matrices
    for j=1:robot.n
        A{j} = robot.links(j).A(q(j)).T * Tpost(:,:,j);
    end

    switch which
        case 'left'
            left = T;
            for j=1:n
                left = inv(A{j}) * left ;
            end

            right = eye(4,4);
            for j=n+1:robot.n
                right = right * A{j};
            end

        case 'right'
            left = T;
            for j=1:n
                left = left * inv(A{robot.n-j+1});
            end

            right = eye(4,4);
            for j=1:(robot.n-n)
                right = right * A{j};
            end
    end

    %     left = simple(left);
    %     right = simple(right);

    if nargout == 0
        left == right
    elseif nargout == 1
        L = left;
    elseif nargout == 2
        L = left;
        R = right;
    end
end

%SOLVE_JOINT Solve a trigonometric equation
%
% S = SOLVE_JOINT(EQ, J) solves the equation EQ=0 for the joint variable qJ.
% The result is a vector of symbolic solutions.
%
% The equations must be of the form:
%  A cos(qJ) + B sin(qJ) = 0
%  A cos(qJ) + B sin(qJ) = C
%
% where A, B, C are arbitrarily complex expressions.  qJ can be the only
% joint variable in the expression.
%
% Notes::
% - In general there are two solutions, but if A^2+B^2-C^2 = 0, then only one
%   solution is returned.
% - The one solution case may not be detected symbolically, in which case
%   the two returned solutions will have the same value after numerical
%   substitution
% - The symbolic solution may not be evaluteable numerically with
%   certain parameter values, ie. if A^2+B^2-C^2 < 0

function s = solve_joint(eq, j)

    % see http://petercorke.com/wordpress/solving-trigonometric-equations

    sinj = mvar('sin(q%d)', j);
    cosj = mvar('cos(q%d)', j);

    A = getcoef(eq, cosj);
    B = getcoef(eq, sinj);

    if isempty(A) || isempty(B)
        warning('don''t know how to solve this kind of equation');
    end

    C = -simplify(eq - A*cosj - B*sinj);

    A = simplify_sumsq(A, j);
    B = simplify_sumsq(B, j);
    C = simplify_sumsq(C, j);

    fprintf('A = %s\n', char(A));
    fprintf('B = %s\n', char(B));
    fprintf('C = %s\n', char(C));

    if C == 0
        fprintf('Solve for C == 0\n');
        % A cos(q) + B sin(q) = 0
        s(1) = atan2(A, -B);
        s(2) = atan2(-A, B);
    else
        fprintf('Solve for C != 0\n');
        % A cos(q) + B sin(q) = C
%         r = sqrt(A^2 + B^2 - C^2);
% %         phi = atan2(A, B);
% %
% %         s(2) = atan2(C, r) - phi;
% %         s(1) = atan2(C, -r) - phi;
%         if r == 0
%             s = atan2(B*C, A*C)
%         else
%             s(1) = atan2(B*C + A*r, A*C - B*r);
%             s(2) = atan2(B*C - A*r, A*C + B*r);
%         end
        d = sqrt( simplify(B^2-C^2+A^2) );
        fprintf('d = %s\n', char(d));

        s(1) = atan2(B*C + A*d, A*C - B*d);
        s(2) = atan2(B*C - A*d, A*C + B*d);
    end

%     simplify( A*cos(s(1)) + B*sin(s(1)) - C )
%     simplify( A*cos(s(2)) + B*sin(s(2)) - C )

    if nargout == 0
        try
            eval(s)
        catch
            s
        end
    end
end

function Qout = simplify_sumsq(Q, N)

    tsubOld = [];
    tsubNew = [];
    for j=1:N
        tsubOld = [tsubOld mvar('S%d', j)^2+mvar('C%d', j)^2];
        tsubNew = [tsubNew         1                        ];
    end
    Q = simplify( rsubs(Q, tsubOld, tsubNew) );
    Qout = simplify( rsubs(Q, tsubOld, tsubNew) );
end

function Qout = simplify_powers(Q, N, trigsubOld, trigsubNew)

        % create a list of simplifications
    %  substitute S^2 = 1-C^2, S^4=(1-C^2)^2
    fprintf('power simplification\n');
    tsubOld = [];
    tsubNew = [];
    for j=1:N
        tsubOld = [tsubOld mvar('S%d', j)^2+mvar('C%d', j)^2 mvar('S%d', j)^4];
        tsubNew = [tsubNew         1                        (1-mvar('C%d', j)^2)^2];
    end

    for j=1:length(Q)
        for k=1:5
            % seem to need to iterate this, not quite sure why
            Q{j} = simplify( expand( subs(Q{j}, tsubOld, tsubNew) ) );
        end
        % subs Sx, Cx to sin(qx), cos(qx)
        Qout{j} = simplify( subs(Q{j}, trigsubOld, trigsubNew) );
    end
end

function out = rsubs(eq, old, new)
    for i = 1:length(old)
        eq = subs(eq, old(i), new(i));
    end
    out = eq;
end

%MVAR Create a symbolic variable
%
% V = MVAR(FMT, ARGS) is a symbolic variable created using SPRINTF
%
% eg. mvar('q%d', j)
%
% The symbolic is explicitly declared to be real.

function v = mvar(fmt, varargin)

    if isempty(strfind(fmt, '('))
        % not a function
        v = sym( sprintf(fmt, varargin{:}), 'real' );
    else
        v = str2sym( sprintf(fmt, varargin{:}) );

    end
end

%HASONLY Determine if an expression contains only certain joint variables
%
% S = HASONLY(E L) is true if the joint variables (q1, q2 etc.) in the expression E
% are listed in the vector L.
%
% Eg. hasonly('sin(q1)*cos(q2)*cos(q4)', [1 2 3]) -> true
% Eg. hasonly('sin(q1)*cos(q2)*cos(q4)', [1]) -> false

function s = hasonly(eq, j)

    q = findq(eq);
    if isempty(q)
        s = false;
    else
        s = all(ismember(j, findq(eq)));
    end
end

%ISCONSTANT Determine if an expression is free of joint variables
%
% S = ISCONSTANT(E) is true if the expression E contains no joint variables such
% q1, q2 etc.

function s = isconstant(eq)
    s = isempty(findq(eq));
end

%FINDQ Find the joint variables in expression
%
% Q = FINDQ(E) returns a list of integers indicating the joint variables found
% in the expression E.  For instance an instance of 'q1' would cause a 1 to be
% returned and so on.
%
% Eg. findq('sin(q1)*cos(q2)+S3') -> [1 2]

function q = findq(s)

    q = [];

    for var=symvar(s)
        if isempty(var)
            break
        end
        varname = char(var);
        if varname(1) == 'q'
            q = [q str2num(varname(2:end))];
        end
    end
end

function coef = getcoef(eq, trig)
    z = children( collect(eq, trig) );
    z = children( z(1) );
    coef = z(1);
end

% Output a joint expression to a file

function s = gencode(Q, filename)

    function s = G(s, fmt, varargin)
        s = strvcat(s, sprintf(fmt, varargin{:}));
    end

    s = 'function q = xikine(T, sol)';
    s = G(s, '  if nargin < 2; sol = ones(1, %d); end', length(Q));
    s = G(s, '  px = T(1,4); py = T(2,4); pz = T(3,4);');

    for j=1:3
        Qj = Q{j};   % cast it to subclass
        if length(Qj) == 1
            s = G(s, '  q(%d) = %s', j, matgen2(Qj));
        elseif length(Qj) == 2
            s = G(s, '  if sol(%d) == 1', j);
            s = G(s, '    q(%d) = %s', j, matgen2(Qj(1)));
            s = G(s, '  else');
            s = G(s, '    q(%d) = %s', j, matgen2(Qj(2)));
            s = G(s, '  end');

        end

        s = G(s, '  S%d = sin(q(%d));', j, j);
        s = G(s, '  C%d = cos(q(%d));', j, j);
        s = G(s, ' ');

    end
    s = G(s, 'end');

    fp = fopen(filename, 'w');
    for i=1:numrows(s)
        fprintf(fp, '%s\n', deblank(s(i,:)));
    end
    fclose(fp);

end

% Generate MATLAB code from an expression
%
% Requires a bit of a hack, a subclass of sym (sym2) to do this

function s = matgen2(e)

    s = matgen(sym2(e));

    k = strfind(s, '=');
    s = deblank( s(k+2:end) );
end