%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Setting up advection-diffusion solver
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [C,laplacian_C] = please_Update_Adv_Diff_Concentration_Flux_Limiter_FV(C,dt,dx,dy,uX,uY,k)

% C:     concentration
% dt:    time-step
% dx,dy: spatial steps in x and y, respectively
% uX:    x-Component of Velocity
% uY:    y-Component of Velocity
% k:     diffusion coefficient

% Compute Fluxes (Note: these calculations could be parallalized)
selection = 'superbee';
Fx = give_Necessary_Fluxes(C,dx,uX,'x',selection,dt); % Fluxes in x
Fy = give_Necessary_Fluxes(C,dy,uY,'y',selection,dt); % Fluxes in y

% "forward difference of fluxes in x"
F2x = [Fx(:,2:end) Fx(:,1)];
F1x = [Fx(:,end) Fx(:,1:end-1)];
diffX = 0.5/dx*( F2x - F1x );

% "forward differences of fluxes in y"
F2y = [Fy(2:end,:); Fy(1,:)];
F1y = [Fy(end,:); Fy(1:end-1,:)];
diffY = 0.5/dy*( F2y - F1y );

% Compute 2nd Derivative Terms
Cxx = DD(C,dx,'x');
Cyy = DD(C,dy,'y');

% Forms Laplacian
laplacian_C = Cxx+Cyy;

% UPWIND
C = C - dt * ( diffX + diffY ) + dt*( k*laplacian_C );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Computes derivative based on sign of Velocity, u, using UPWIND
% approach
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function C_z = give_Necessary_Fluxes(C,dz,uZ,string,selection,dt)

C_z = zeros(size(C));
len = length(uZ(:,1));
signs = sign(uZ);

if strcmp(string,'x')

    %For periodicity on ends w/ UPWIND
    for i=1:len

        %left side of grid
        if signs(i,1) <= 0
            r = ( C(i,2) - C(i,1) ) / ( C(i,1) - C(i,len) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(i,1) =  uZ(i,1)*C(i,2)   + 0.5*abs( uZ(i,1) )*( 1 - abs( 0.5*dt*uZ(i,1)/dz ) )*phi*( C(i,2) - C(i,len) );
        else
            r = ( C(i,1) - C(i,len) ) / ( C(i,2) - C(i,1) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(i,1) =  uZ(i,1)*C(i,len) + 0.5*abs( uZ(i,1) )*( 1 - abs( 0.5*dt*uZ(i,1)/dz ) )*phi*( C(i,2) - C(i,len) );
        end

        %right side of grid
        if signs(len,1) <= 0
            r = ( C(i,1) - C(i,len) ) / ( C(i,len) - C(i,len-1) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(i,len) =  uZ(i,len)*C(i,1)     + 0.5*abs( uZ(i,len) )*( 1 - 0.5*abs( dt*uZ(i,len)/dz ) )*phi*( C(i,1) - C(i,len-1) );
        else
            r = ( C(i,len) - C(i,len-1) ) / ( C(i,1) - C(i,len) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(i,len) =  uZ(i,len)*C(i,len-1) + 0.5*abs( uZ(i,len) )*( 1 - 0.5*abs( dt*uZ(i,len)/dz ) )*phi*( C(i,1) - C(i,len-1) );
        end

    end
    %Standard Upwind
    for i=1:len
        for j=2:len-1
            if signs(i,j) <= 0
                r = ( C(i,j+1) - C(i,j) ) / ( C(i,j) - C(i,j-1) );
                phi = please_Give_Flux_Limiter(r,selection);
                C_z(i,j) = uZ(i,j)*C(i,j+1) + 0.5*abs( uZ(i,j) )*( 1 - 0.5*abs( dt*uZ(i,j)/dz ) )*phi*( C(i,j+1) - C(i,j-1) );
            else
                r = ( C(i,j) - C(i,j-1) ) / ( C(i,j+1) - C(i,j) );
                phi = please_Give_Flux_Limiter(r,selection);
                C_z(i,j) = uZ(i,j)*C(i,j-1) + 0.5*abs( uZ(i,j) )*( 1 - 0.5*abs( dt*uZ(i,j)/dz ) )*phi*( C(i,j+1) - C(i,j-1) );
            end
        end
    end

    % Ends x-Direction calculation %

elseif strcmp(string,'y')

    %For periodicity on ends w/ UPWIND
    for i=1:len

        %bottom of grid
        if signs(1,i) <= 0
            r = ( C(2,i) - C(1,i) ) / ( C(1,i) - C(len,i) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(1,i) =  uZ(1,i)*C(2,i)   + 0.5*abs( uZ(1,i) )*( 1 - abs( 0.5*dt*uZ(1,i)/dz ) )*phi*( C(2,i) - C(len,i) );
        else
            r = ( C(1,i) - C(len,i) ) / ( C(2,i) - C(1,i) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(1,i) =  uZ(1,i)*C(len,i) + 0.5*abs( uZ(1,i) )*( 1 - abs( 0.5*dt*uZ(1,i)/dz ) )*phi*( C(2,i) - C(len,i) );
        end

        %top of grid
        if signs(len,1) <= 0
            r = ( C(1,i) - C(len,i) ) / ( C(len,i) - C(len-1,i) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(len,i) =  uZ(len,i)*C(1,i)       + 0.5*abs( uZ(len,i) )*( 1 - abs( 0.5*dt*uZ(len,i)/dz ) )*phi*( C(1,i) - C(len-1,i) );
        else
            r = ( C(len,i) - C(len-1,i) ) / ( C(1,i) - C(len,i) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(len,i) =  uZ(len,i)*C(len-1,i)   + 0.5*abs( uZ(len,i) )*( 1 - abs( 0.5*dt*uZ(len,i)/dz ) )*phi*( C(1,i) - C(len-1,i) );
        end

    end

    %Standard Upwind
    for i=1:len
        for j=2:len-1
            if signs(j,i) <= 0
                r = ( C(j+1,i) - C(j,i) ) / ( C(j,i) - C(j-1,i) );
                phi = please_Give_Flux_Limiter(r,selection);
                C_z(j,i) = uZ(j,i)*C(j+1,i) + 0.5*abs( uZ(j,i) )*( 1 - abs( 0.5*dt*uZ(j,i)/dz ) )*phi*( C(j+1,i) - C(j-1,i) );
            else
                r = ( C(j,i) - C(j-1,i) ) / ( C(j+1,i) - C(j,i) );
                phi = please_Give_Flux_Limiter(r,selection);
                C_z(j,i) = uZ(j,i)*C(j-1,i) + 0.5*abs( uZ(j,i) )*( 1 - abs( 0.5*dt*uZ(j,i)/dz ) )*phi*( C(j+1,i) - C(j-1,i) );
            end
        end
    end

    % Ends y-Direction calculation %

else

    fprintf('\n\n\n ERROR IN FUNCTION FOR COMPUTING FLUX LIMITERS\n');

end

clear signs; clear len;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes flux limiter with choice of which one
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function phi = please_Give_Flux_Limiter(r,selection)

if strcmp(selection,'superbee')
    max1 = max( min(1,2*r),min(2,r) );
    phi = max(0,max1);
else
    fprintf('\n\n');
    error('NEED TO CHOOSE AN APPROPRIATE FLUX LIMITER');
end


function [map, dist] = imPointsInfluenceZones(varargin)
%IMPOINTSINFLUENCEZONES Maps influence zones of a set of 2D/3D points
%
%   MAP = imPointsInfluenceZones(DIM, POINTS)
%   DIM is a 1-by-2 or 1-by-3 row vector containing dimensions of ouput
%   label map. POINTS is a N-by-2 or N-by-3 array of coordinates.
%
%   MAP = imPointsInfluenceZones(LX, LY, POINTS)
%   MAP = imPointsInfluenceZones(LX, LY, LZ, POINTS)
%   LX and LY and optionnaly LZ are row vectors containing the values of
%   XData, YData, and ZData. The size of MAP if given by:
%   * length(LY)-by-length(LX) in case of 2D points
%   * length(LY)-by-length(LX)-by-length(LZ) in case of 3D points
%
%   [MAP, DIST] = imPointsInfluenceZones(...)
%   Also returns the distance map, containing for each pixel or voxel the
%   distance to the closest point in the input point set.
%
%   Example
%     % Planar example
%     points = rand(20, 2) * 200;
%     map = imPointsInfluenceZones([200 200], points);
%     rgb = label2rgb(map, jet(20), 'w', 'shuffle');
%     figure; imshow(rgb);
%
%     % 3D example
%     dim = [100 100 100];
%     np = 200;
%     points = rand(np, 3) * dim(1);
%     tic; [map, dist] = imPointsInfluenceZones(dim, points); toc
%     rgb = label2rgb3d(map, jet(np+1), 'w', 'shuffle');
%     figure; imshow(rgb(:,:,:,50));
%
%   See also
%     imDistance, imvoronoi2d, imvoronoi3d

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2019-05-10,    using Matlab 9.6.0.1072779 (R2019a)
% Copyright 2019 INRA - Cepia Software Platform.

%% Extract input arguments

% checkup on input argument number
if nargin < 2
    error('Requires at least two input arguments');
end

% extraction of image dimensions
var1 = varargin{1};
if size(var1, 1) == 1 && (size(var1, 2) == 2 || size(var1, 2) == 3)
    % first argument contains the size of the output image
    lx = 1:var1(2);
    ly = 1:var1(1);
    if length(var1) == 3
        lz = 1:var1(3);
    end
    varargin(1) = [];

elseif size(var1, 1) == 1 && size(varargin{2}, 1) == 1
    % first and second arguments contain vector for each coordinate
    % respectively
    lx = var1;
    ly = varargin{2};
    if size(varargin{3}, 1) == 1
        lz = varargin{3};
        varargin(1:3) = [];
    else
        varargin(1:2) = [];
    end

else
    error(['wrong input arguments in ' mfilename]);
end

% extraction of points
points = varargin{1};

%% Initialisations

% number of points
nPoints = size(points, 1);
nd = size(points, 2);

% size of output image
Nx = length(lx);
Ny = length(ly);

%% Generation of distance function

if nd == 2
    % allocate memory for label map
    map = zeros([Ny Nx]);
    % initialize distance map with arbitrarily large distance
    dist = inf * ones([Ny Nx]);

    % pixels coordinates
    [x, y] = meshgrid(lx, ly);

    % update distance for each point
    for i = 1:nPoints
        % squared distance from each pixel to current point
        di = (x - points(i,1)).^2 + (y - points(i,2)).^2;

        % update arrays for current influence zone
        inds = di < dist;
        map(inds) = i;
        dist(inds) = di(inds);
    end

    % convert squared distance to Euclidean distance
    dist = sqrt(dist);

elseif nd == 3
    % size in third dimension
    Nz = length(lz);

    % allocate memory for label map
    map = zeros([Ny Nx Nz]);

    % initialize distance map with arbitrarily large distance
    dist = inf * ones([Ny Nx Nz]);

    % pixels coordinates
    [x, y, z] = meshgrid(lx, ly, lz);

    % update distance for each point
    for i = 1:nPoints
        % distance from each pixel to current point
        di = (x - points(i,1)).^2 + (y - points(i,2)).^2 + (z - points(i,3)).^2;

        % update arrays for current influence zone
        inds = di < dist;
        map(inds) = i;
        dist(inds) = di(inds);
    end

    % convert squared distance to Euclidean distance
    dist = sqrt(dist);
end


%% ------------------------solveRWF.m--------------------------------------

% Solver for Reweighted Wirtinger Flow as given in Algorithm 1
% of the Reweightde Wirtinger Flow (TAF) paper. Refer to the userguide for
% a detailed usage of the package.

% PAPER TITLE:
%              Phase Retrieval via Reweighted Wirtinger Flow
% ARXIV LINK:
%              https://doi.org/10.1364/AO.56.002418

% INPUTS:
%         A:   Function handle/numerical matrix for data matrix A. The rows
%              of this matrix are the measurement vectors that produce
%              amplitude measurements '\psi'.
%         At:  Function handle/numerical matrix for A transpose.
%         b0:  Observed data vector consisting of amplitude measurements
%              generated from b0 = |A*x|. We assign it to 'psi' to be
%              consistent with the notation in the paper.
%         x0:  The initial vector to be used by any solver.
%        opts: struct consists of the options for the algorithm. For
%              details,see header in solvePhaseRetrieval.m or the User
%              Guide.

% OUPTUT :
%         sol: n x 1 vector. It is the estimated signal.
%        outs: A struct consists of the convergence info. For details,
%              see header in solvePhaseRetrieval.m or the User Guide.

% Note:        When a function handle is used, the value of 'n' (the length
%              of the unknown signal) and 'At' (a function handle for the
%              adjoint of 'A') must be supplied. When 'A' is numeric, the
%              values of 'At' and 'n' are ignored and inferred from the
%              arguments

% DESCRIPTION:
%             The spectral method proposed by Candes et al. in their
%             seminal Wirtinger Flow paper suffers from fat tails that can
%             distort the true solution. A possible way to overcome this is
%             to truncate the outliers. In this paper, the authors propose
%             a weighted truncated iterative descent method that removes
%             those measurement vector a_m that are not correlated to the
%             intial guess. This truncation and weighting step is done at
%             each iteration of the gradient descent method.

% METHOD:
%         1) Our implementation uses FASTA, a fast gradient solver.
%
%         2) Set the objectve f = @(z) 1/2 * sum(weights .*
%             (abs(z) - b0).^2). The weights are computed according to
%             equation (14) in algorithm 1 of the proposing paper
%
%         3) The gradient is grad = @(z) weights .* (z - b0 .* sign(z)).
%
% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

%% -----------------------------START-----------------------------------

function [sol, outs] = solveRWF(A, At, b0, x0, opts)
%     addpath('solvers/linesearch');

    m = length(b0);

    innerOpts = struct;
    innerOpts.maxIters = opts.maxIters;
    innerOpts.maxTime = opts.maxTime;
    innerOpts.tol = opts.tol;
    innerOpts.verbose = opts.verbose;
    innerOpts.recordTimes = opts.recordTimes;
    innerOpts.recordResiduals = opts.recordResiduals;
    innerOpts.recordMeasurementErrors = opts.recordMeasurementErrors;
    innerOpts.recordReconErrors = opts.recordReconErrors;
    innerOpts.xt = opts.xt;

    innerOpts.updateObjectivePeriod = opts.reweightPeriod;
    innerOpts.searchMethod = opts.searchMethod;
    innerOpts.betaChoice = opts.betaChoice;

    [sol, outs] = gradientDescentSolver(A, At, x0, b0, @updateObjective, innerOpts);

    function [f, gradf] = updateObjective(~, Ax)
        weights = 1 ./ (abs(abs(Ax).^2 - b0.^2) + opts.eta*ones(m, 1));
        s = sum(weights);
        f = @(z) 0.5/s * sum(weights .* (abs(z).^2 - b0.^2).^2);
        gradf = @(z) (1.0/s)*weights .* (abs(z).^2 - b0.^2) .* z;
    end
end


function W = randInitializeWeights(L_in, L_out)
%RANDINITIALIZEWEIGHTS Randomly initialize the weights of a layer with L_in
%incoming connections and L_out outgoing connections
%   W = RANDINITIALIZEWEIGHTS(L_in, L_out) randomly initializes the weights
%   of a layer with L_in incoming connections and L_out outgoing
%   connections.
%
%   Note that W should be set to a matrix of size(L_out, 1 + L_in) as
%   the first column of W handles the "bias" terms
%

% You need to return the following variables correctly
W = zeros(L_out, 1 + L_in);

% ====================== YOUR CODE HERE ======================
% Instructions: Initialize W randomly so that we break the symmetry while
%               training the neural network.
%
% Note: The first column of W corresponds to the parameters for the bias unit
%

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

end


function [m,v,w,g,f,pp,gg]=gaussmix(x,c,l,m0,v0,w0)
%GAUSSMIX fits a gaussian mixture pdf to a set of data observations [m,v,w,g,f]=(x,c,l,m0,v0,w0)
%
% Inputs: n data values, k mixtures, p parameters, l loops
%
%     X(n,p)   Input data vectors, one per row.
%     c(1)     Minimum variance of normalized data (Use [] to take default value of 1/n^2)
%     L        The integer portion of l gives a maximum loop count. The fractional portion gives
%              an optional stopping threshold. Iteration will cease if the increase in
%              log likelihood density per data point is less than this value. Thus l=10.001 will
%              stop after 10 iterations or when the increase in log likelihood falls below
%              0.001.
%              As a special case, if L=0, then the first three outputs are omitted.
%              Use [] to take default value of 100.0001
%     M0(k,p)  Initial mixture means, one row per mixture.
%     V0(k,p)  Initial mixture variances, one row per mixture.
%      or V0(p,p,k)  one full-covariance matrix per mixture
%     W0(k,1)  Initial mixture weights, one per mixture. The weights should sum to unity.
%
%     Alternatively, if initial values for M0, V0 and W0 are not given explicitly:
%
%     M0       Number of mixtures required
%     V0       Initialization mode:
%                'f'    Initialize with K randomly selected data points [default]
%                'p'    Initialize with centroids and variances of random partitions
%                'k'    k-means algorithm ('kf' and 'kp' determine initialization of kmeans)
%                'h'    k-harmonic means algorithm ('hf' and 'hp' determine initialization of kmeans)
%                's'    do not scale data during initialization to have equal variances
%                'm'    M0 contains the initial centres
%                'v'    full covariance matrices
%              Mode 'hf' [the default] generally gives the best results but 'f' is faster and often OK
%
% Outputs: (Note that M, V and W are omitted if L==0)
%
%     M(k,p)   Mixture means, one row per mixture. (omitted if L==0)
%     V(k,p)   Mixture variances, one row per mixture. (omitted if L==0)
%       or V(p,p,k) if full covariance matrices in use (i.e. either 'v' option or V0(p,p,k) specified)
%     W(k,1)   Mixture weights, one per mixture. The weights will sum to unity. (omitted if L==0)
%     G       Average log probability of the input data points.
%     F        Fisher's Discriminant measures how well the data divides into classes.
%              It is the ratio of the between-mixture variance to the average mixture variance: a
%              high value means the classes (mixtures) are well separated.
%     PP(n,1)  Log probability of each data point
%     GG(l+1,1) Average log probabilities at the beginning of each iteration and at the end

%  Bugs/Suggestions
%     (2) Allow processing in chunks by outputting/reinputting an array of sufficient statistics
%     (6) Other initialization options:
%              'l'    LBG algorithm
%              'm'    Move-means (dog-rabbit) algorithm

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

[n,p]=size(x);
mx0=sum(x,1)/n;         % calculate mean and variance of input data
vx0=sum(x.^2,1)/n-mx0.^2;
sx0=sqrt(vx0);
sx0(sx0==0)=1;      % do not divide by zero when scaling
scaled=0;           % data is not yet scaled
memsize=voicebox('memsize');    % set memory size to use
if isempty(c)
    c=1/n^2;
else
    c=c(1);         % just to prevent legacy code failing
end
fulliv=0;           % initial variance is not full
if isempty(l)
    l=100+1e-4;         % max loop count + stopping threshold
end
if nargin<6             % no initial values specified for m0, v0, w0
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %  No initialvalues given, so we must use k-means or equivalent
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if nargin<5
        v0='hf';                 % default initialization mode: hf
    end
    if any(v0=='m')
        k=size(m0,1);
    else
        k=m0;
    end
    fv=any(v0=='v');                % full covariance matrices requested
    if n<=k                         % each data point can have its own mixture
        xs=(x-mx0(ones(n,1),:))./sx0(ones(n,1),:);          % scale the data
        m=xs(mod((1:k)-1,n)+1,:);    % just include all points several times
        v=zeros(k,p);               % will be set to floor later
        w=zeros(k,1);
        w(1:n)=1/n;
        if l>0
            l=0.1;                  % no point in iterating
        end
    else                            % more points than mixtures
        if any(v0=='s')
            xs=x;                   % do not scale data during initialization
        else
            xs=(x-mx0(ones(n,1),:))./sx0(ones(n,1),:);  % else scale now
            if any(v0=='m')
                m=(m0-mx0(ones(k,1),:))./sx0(ones(k,1),:);  % scale specified means as well
            end
        end
        w=repmat(1/k,k,1);                      % all mixtures equally likely
        if any(v0=='k')                         % k-means initialization
            if any(v0=='m')
                [m,e,j]=kmeans(xs,k,m);
            elseif any(v0=='p')
                [m,e,j]=kmeans(xs,k,'p');
            else
                [m,e,j]=kmeans(xs,k,'f');
            end
        elseif any(v0=='h')                     % k-harmonic means initialization
            if any(v0=='m')
                [m,e,j]=kmeanhar(xs,k,[],4,m);
            else
                if any(v0=='p')
                    [m,e,j]=kmeanhar(xs,k,[],4,'p');
                else
                    [m,e,j]=kmeanhar(xs,k,[],4,'f');
                end
            end
        elseif any(v0=='p')                     % Initialize using a random partition
            j=ceil(rand(n,1)*k);                % allocate to random clusters
            j(rnsubset(k,n))=1:k;               % but force at least one point per cluster
            for i=1:k
                m(i,:)=mean(xs(j==i,:),1);
            end
        else
            if any(v0=='m')
                m=m0;                           % use specified centres
            else
                m=xs(rnsubset(k,n),:);          % Forgy initialization: sample k centres without replacement [default]
            end
            [e,j]=kmeans(xs,k,m,0);             % find out the cluster allocation
        end
        if any(v0=='s')
            xs=(x-mx0(ones(n,1),:))./sx0(ones(n,1),:);      % scale data now if not done previously
        end
        v=zeros(k,p);                   % diagonal covariances
        w=zeros(k,1);
        for i=1:k
            ni=sum(j==i);               % number assigned to this centre
            w(i)=(ni+1)/(n+k);          % weight of this mixture
            if ni
                v(i,:)=sum((xs(j==i,:)-repmat(m(i,:),ni,1)).^2,1)/ni;
            else
                v(i,:)=zeros(1,p);
            end
        end
    end
else
    %%%%%%%%%%%%%%%%%%%%%%%%
    % use initial values given as input parameters
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    [k,p]=size(m0);
    xs=(x-mx0(ones(n,1),:))./sx0(ones(n,1),:);          % scale the data
    m=(m0-mx0(ones(k,1),:))./sx0(ones(k,1),:);          % and the means
    v=v0;
    w=w0;
    fv=ndims(v)>2 || size(v,1)>k;                       % full covariance matrix is supplied
    if fv
        mk=eye(p)==0;                                    % off-diagonal elements
        fulliv=any(v(repmat(mk,[1 1 k]))~=0);            % check if any are non-zero
        if ~fulliv
            v=reshape(v(repmat(~mk,[1 1 k])),p,k)'./repmat(sx0.^2,k,1);   % just pick out and scale the diagonal elements for now
        else
            v=v./repmat(sx0'*sx0,[1 1 k]);              % scale the full covariance matrix
        end
    end
end
lsx=sum(log(sx0));
if ~fulliv          % initializing with diagonal covariance
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Diagonal Covariance matrices  %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    v=max(v,c);         % apply the lower bound
    xs2=xs.^2;          % square the data for variance calculations

    % If data size is large then do calculations in chunks

    nb=min(n,max(1,floor(memsize/(8*p*k))));    % chunk size for testing data points
    nl=ceil(n/nb);                  % number of chunks
    jx0=n-(nl-1)*nb;                % size of first chunk

    im=repmat(1:k,1,nb); im=im(:);
    th=(l-floor(l))*n;
    sd=(nargout > 3*(l~=0)); % = 1 if we are outputting log likelihood values
    lp=floor(l)+sd;   % extra loop needed to calculate final G value

    lpx=zeros(1,n);             % log probability of each data point
    wk=ones(k,1);
    wp=ones(1,p);
    wnb=ones(1,nb);
    wnj=ones(1,jx0);

    % EM loop

    g=0;                           % dummy initial value for comparison
    gg=zeros(lp+1,1);
    ss=sd;                       % initialize stopping count (0 or 1)
    for j=1:lp
        g1=g;                    % save previous log likelihood (2*pi factor omitted)
        m1=m;                       % save previous means, variances and weights
        v1=v;
        w1=w;
        vi=-0.5*v.^(-1);                % data-independent scale factor in exponent
        lvm=log(w)-0.5*sum(log(v),2);   % log of external scale factor (excluding -0.5*p*log(2pi) term)

        % first do partial chunk

        jx=jx0;
        ii=1:jx;
        kk=repmat(ii,k,1);
        km=repmat(1:k,1,jx);
        py=reshape(sum((xs(kk(:),:)-m(km(:),:)).^2.*vi(km(:),:),2),k,jx)+lvm(:,wnj);
        mx=max(py,[],1);                % find normalizing factor for each data point to prevent underflow when using exp()
        px=exp(py-mx(wk,:));            % find normalized probability of each mixture for each datapoint
        ps=sum(px,1);                   % total normalized likelihood of each data point
        px=px./ps(wk,:);                % relative mixture probabilities for each data point (columns sum to 1)
        lpx(ii)=log(ps)+mx;
        pk=sum(px,2);                   % effective number of data points for each mixture (could be zero due to underflow)
        sx=px*xs(ii,:);
        sx2=px*xs2(ii,:);

        for il=2:nl
            ix=jx+1;
            jx=jx+nb;                    % increment upper limit
            ii=ix:jx;
            kk=repmat(ii,k,1);
            py=reshape(sum((xs(kk(:),:)-m(im,:)).^2.*vi(im,:),2),k,nb)+lvm(:,wnb);
            mx=max(py,[],1);                % find normalizing factor for each data point to prevent underflow when using exp()
            px=exp(py-mx(wk,:));            % find normalized probability of each mixture for each datapoint
            ps=sum(px,1);                   % total normalized likelihood of each data point
            px=px./ps(wk,:);                % relative mixture probabilities for each data point (columns sum to 1)
            lpx(ii)=log(ps)+mx;
            pk=pk+sum(px,2);                   % effective number of data points for each mixture (could be zero due to underflow)
            sx=sx+px*xs(ii,:);
            sx2=sx2+px*xs2(ii,:);
        end
        g=sum(lpx);                    % total log probability summed over all data points
        gg(j)=g;
        w=pk/n;                         % normalize to get the weights
        if pk                       % if all elements of pk are non-zero
            m=sx./pk(:,wp);
            v=sx2./pk(:,wp);
        else
            wm=pk==0;                       % mask indicating mixtures with zero weights
            nz=sum(wm);                  % number of zero-weight mixtures
            [vv,mk]=sort(lpx);             % find the lowest probability data points
            m=zeros(k,p);                   % initialize means and variances to zero (variances are floored later)
            v=m;
            m(wm,:)=xs(mk(1:nz),:);                % set zero-weight mixture means to worst-fitted data points
            w(wm)=1/n;                      % set these weights non-zero
            w=w*n/(n+nz);                   % normalize so the weights sum to unity
            wm=~wm;                         % mask for non-zero weights
            m(wm,:)=sx(wm,:)./pk(wm,wp);  % recalculate means and variances for mixtures with a non-zero weight
            v(wm,:)=sx2(wm,:)./pk(wm,wp);
        end
        v=max(v-m.^2,c);                % apply floor to variances

        if g-g1<=th && j>1
            if ~ss, break; end  %  stop
            ss=ss-1;       % stop next time
        end

    end
    if sd && ~fv  % we need to calculate the final probabilities
        pp=lpx'-0.5*p*log(2*pi)-lsx;   % log of total probability of each data point
        gg=gg(1:j)/n-0.5*p*log(2*pi)-lsx;    % average log prob at each iteration
        g=gg(end);
        %     gg' % *** DEBUG ***
        m=m1;       % back up to previous iteration
        v=v1;
        w=w1;
        mm=sum(m,1)/k;
        f=(m(:)'*m(:)-k*mm(:)'*mm(:))/sum(v(:));
    end
    if ~fv
        m=m.*sx0(ones(k,1),:)+mx0(ones(k,1),:);  % unscale means
        v=v.*repmat(sx0.^2,k,1);                % and variances
    else
        v1=v;
        v=zeros(p,p,k);
        mk=eye(p)==1;                                    % mask for diagonal elements
        v(repmat(mk,[1 1 k]))=v1';            % set from v1
    end
end
if fv              % check if full covariance matrices were requested
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Full Covariance matrices  %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    pl=p*(p+1)/2;
    lix=1:p^2;
    cix=repmat(1:p,p,1);
    rix=cix';
    lix(cix>rix)=[];                                        % index of lower triangular elements
    cix=cix(lix);                                           % index of lower triangular columns
    rix=rix(lix);                                           % index of lower triangular rows
    dix=find(rix==cix);
    lixi=zeros(p,p);
    lixi(lix)=1:pl;
    lixi=lixi';
    lixi(lix)=1:pl;                                        % reverse index to build full matrices
    v=reshape(v,p^2,k);
    v=v(lix,:)';                                            % lower triangular in rows

    % If data size is large then do calculations in chunks

    nb=min(n,max(1,floor(memsize/(24*p*k))));    % chunk size for testing data points
    nl=ceil(n/nb);                  % number of chunks
    jx0=n-(nl-1)*nb;                % size of first chunk
    %
    th=(l-floor(l))*n;
    sd=(nargout > 3*(l~=0)); % = 1 if we are outputting log likelihood values
    lp=floor(l)+sd;   % extra loop needed to calculate final G value
    %
    lpx=zeros(1,n);             % log probability of each data point
    wk=ones(k,1);
    wp=ones(1,p);

    wpl=ones(1,pl);       % 1 index for lower triangular matrix
    wnb=ones(1,nb);
    wnj=ones(1,jx0);

    % EM loop

    g=0;                           % dummy initial value for comparison
    gg=zeros(lp+1,1);
    ss=sd;                       % initialize stopping count (0 or 1)
    vi=zeros(p*k,p);                    % stack of k inverse cov matrices each size p*p
    vim=zeros(p*k,1);                   % stack of k vectors of the form inv(v)*m
    mtk=vim;                             % stack of k vectors of the form m
    lvm=zeros(k,1);
    wpk=repmat((1:p)',k,1);
    for j=1:lp
        g1=g;                    % save previous log likelihood (2*pi factor omitted)
        m1=m;                       % save previous means, variances and weights
        v1=v;
        w1=w;

        for ik=1:k

            % these lines added for debugging only
            %             vk=reshape(v(k,lixi),p,p);
            %             condk(ik)=cond(vk);
            %%%%%%%%%%%%%%%%%%%%
            [uvk,dvk]=eig(reshape(v(ik,lixi),p,p));      % convert lower triangular to full and find eigenvalues
            dvk=max(diag(dvk),c);                           % apply variance floor to eigenvalues
            vik=-0.5*uvk*diag(dvk.^(-1))*uvk';   % calculate inverse
            vi((ik-1)*p+(1:p),:)=vik;           % vi contains all mixture inverses stacked on top of each other
            vim((ik-1)*p+(1:p))=vik*m(ik,:)';   % vim contains vi*m for all mixtures stacked on top of each other
            mtk((ik-1)*p+(1:p))=m(ik,:)';       % mtk contains all mixture means stacked on top of each other
            lvm(ik)=log(w(ik))-0.5*sum(log(dvk));       % vm contains the weighted sqrt of det(vi) for each mixture
        end
        %
        %         % first do partial chunk
        %
        jx=jx0;
        ii=1:jx;
        xii=xs(ii,:).';
        py=reshape(sum(reshape((vi*xii-vim(:,wnj)).*(xii(wpk,:)-mtk(:,wnj)),p,jx*k),1),k,jx)+lvm(:,wnj);
        mx=max(py,[],1);                % find normalizing factor for each data point to prevent underflow when using exp()
        px=exp(py-mx(wk,:));  % find normalized probability of each mixture for each datapoint
        ps=sum(px,1);                   % total normalized likelihood of each data point
        px=px./ps(wk,:);                % relative mixture probabilities for each data point (columns sum to 1)
        lpx(ii)=log(ps)+mx;
        pk=sum(px,2);                   % effective number of data points for each mixture (could be zero due to underflow)
        sx=px*xs(ii,:);
        sx2=px*(xs(ii,rix).*xs(ii,cix));            % accumulator for variance calculation (lower tri cov matrix as a row)

        for il=2:nl
            ix=jx+1;
            jx=jx+nb;        % increment upper limit
            ii=ix:jx;
            xii=xs(ii,:).';
            py=reshape(sum(reshape((vi*xii-vim(:,wnb)).*(xii(wpk,:)-mtk(:,wnb)),p,nb*k),1),k,nb)+lvm(:,wnb);
            mx=max(py,[],1);                % find normalizing factor for each data point to prevent underflow when using exp()
            px=exp(py-mx(wk,:));  % find normalized probability of each mixture for each datapoint
            ps=sum(px,1);                   % total normalized likelihood of each data point
            px=px./ps(wk,:);                % relative mixture probabilities for each data point (columns sum to 1)
            lpx(ii)=log(ps)+mx;
            pk=pk+sum(px,2);                % effective number of data points for each mixture (could be zero due to underflow)
            sx=sx+px*xs(ii,:);               % accumulator for mean calculation
            sx2=sx2+px*(xs(ii,rix).*xs(ii,cix));            % accumulator for variance calculation
        end
        g=sum(lpx);                    % total log probability summed over all data points
        gg(j)=g;                        % save convergence history
        w=pk/n;                         % normalize to get the column of weights
        if pk                       % if all elements of pk are non-zero
            m=sx./pk(:,wp);         % find mean and mean square
            v=sx2./pk(:,wpl);
        else
            wm=pk==0;                       % mask indicating mixtures with zero weights
            nz=sum(wm);                  % number of zero-weight mixtures
            [vv,mk]=sort(lpx);             % find the lowest probability data points
            m=zeros(k,p);                   % initialize means and variances to zero (variances are floored later)
            v=zeros(k,pl);
            m(wm,:)=xs(mk(1:nz),:);                % set zero-weight mixture means to worst-fitted data points
            w(wm)=1/n;                      % set these weights non-zero
            w=w*n/(n+nz);                   % normalize so the weights sum to unity
            wm=~wm;                         % mask for non-zero weights
            m(wm,:)=sx(wm,:)./pk(wm,wp);  % recalculate means and variances for mixtures with a non-zero weight
            v(wm,:)=sx2(wm,:)./pk(wm,wpl);
        end
        v=v-m(:,cix).*m(:,rix);                 % subtract off mean squared
        if g-g1<=th && j>1
            if ~ss, break; end  %  stop
            ss=ss-1;       % stop next time
        end
    end
    if sd  % we need to calculate the final probabilities
        pp=lpx'-0.5*p*log(2*pi)-lsx;   % log of total probability of each data point
        gg=gg(1:j)/n-0.5*p*log(2*pi)-lsx;    % average log prob at each iteration
        g=gg(end);
        %             gg' % *** DEBUG ONLY ***
        m=m1;       % back up to previous iteration
        v=zeros(p,p,k);
        trv=0;      % sum of variance matrix traces
        for ik=1:k
            [uvk,dvk]=eig(reshape(v1(ik,lixi),p,p));      % convert lower triangular to full and find eigenvectors
            dvk=max(diag(dvk),c);                       % apply variance floor
            v(:,:,ik)=uvk*diag(dvk)*uvk';          % reconstitute full matrix
            trv=trv+sum(dvk);
        end
        w=w1;
        mm=sum(m,1)/k;
        f=(m(:)'*m(:)-k*mm(:)'*mm(:))/trv;
    else
        v1=v;               % lower triangular form
        v=zeros(p,p,k);
        for ik=1:k
            [uvk,dvk,]=eig(reshape(v1(ik,lixi),p,p));      % convert lower triangular to full and find eigenvectors
            dvk=max(diag(dvk),c);                       % apply variance floor
            v(:,:,ik)=uvk*diag(dvk)*uvk';          % reconstitute full matrix
        end
    end
    m=m.*sx0(ones(k,1),:)+mx0(ones(k,1),:);  % unscale means
    v=v.*repmat(sx0'*sx0,[1 1 k]);
end
if l==0         % suppress the first three output arguments if l==0
    m=g;
    v=f;
    w=pp;
end


function [A,B,flag] = hmxRSVD(varargin)
%+========================================================================+
%|                                                                        |
%|         OPENHMX - LIBRARY FOR H-MATRIX COMPRESSION AND ALGEBRA         |
%|           openHmx is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : hmxRSVD.m                                     |
%|    #    |   VERSION    : 0.52                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 01.01.2019                                    |
%| ( === ) |   SYNOPSIS   : RSVD from Halko et al. 'finding structure with|
%|  `---'  |                randomness'. Adapted from Antoine Liutkus.    |
%+========================================================================+

% Input analysis
if (numel(varargin{2}) == 1)
    % Input
    M   = varargin{1};
    tol = varargin{2};
    rk  = varargin{3};

    % Dimensions
    [m,n] = size(M);

    % Matrix vectot product
    MV = @(V) M * V;
    VM = @(V) V * M;

    % Type
    typ = class(M);

else
    % Input
    A   = varargin{1};
    B   = varargin{2};
    tol = varargin{3};

    % Dimensions
    [m,rk] = size(A);
    n      = size(B,2);

    % Type
    typ = class(A);

    % Matrix vectot product
    MV    = @(V) A * (B * V);
    VM    = @(V) (V * A) * B;
end

% Randomized
p = min(2*rk,n);
X = randn(n,p,typ);
Y = MV(X);
try
    W1 = orth(Y);
catch
    A    = [];
    B    = [];
    flag = 0;
    return
end
B = VM(W1');

% Truncated SVD
try
    [W2,S,V] = svd(B,'econ');
catch
    A    = [];
    B    = [];
    flag = 0;
    return
end

% Product
U = W1*W2;

% Rank with fixed accuracy
if (numel(S) ~= 0)
    I = find(abs(diag(S)/S(1)) >= tol);
else
    I = [];
end

% No values
if isempty(I)
   A    = zeros(m,0);
   B    = zeros(0,n);
   flag = 1;

% Accuracy not reached
elseif (length(I) >= rk)
    A    = [];
    B    = [];
    flag = 0;

% Low-rank representation
else
    A    = U(:,I);
    B    = S(I,I) * V(:,I)';
    flag = 1;
end
end


% loss_sqp - squared loss function
%
% Copyright(c) 2009 Ryota Tomioka
% This software is distributed under the MIT license. See license.txt
function [floss, gloss]=loss_sqp(zz, bb)

gloss = zz-bb;
floss = 0.5*sum(gloss.^2);


%IMM_UPDATE  Interacting Multiple Model (IMM) Filter update step
%
% Syntax:
%   [X_i,P_i,MU,X,P] = IMM_UPDATE(X_p,P_p,c_j,ind,dims,Y,H,R)
%
% In:
%   X_p  - Cell array containing N^j x 1 mean state estimate vector for
%          each model j after prediction step
%   P_p  - Cell array containing N^j x N^j state covariance matrix for
%          each model j after prediction step
%   c_j  - Normalizing factors for mixing probabilities
%   ind  - Indices of state components for each model as a cell array
%   dims - Total number of different state components in the combined system
%   Y    - Dx1 measurement vector.
%   H    - Measurement matrices for each model as a cell array.
%   R    - Measurement noise covariances for each model as a cell array.
%
% Out:
%   X_i  - Updated state mean estimate for each model as a cell array
%   P_i  - Updated state covariance estimate for each model as a cell array
%   MU   - Estimated probabilities of each model
%   X    - Combined state mean estimate
%   P    - Combined state covariance estimate
%
% Description:
%   IMM filter measurement update step.
%
% See also:
%   IMM_PREDICT, IMM_SMOOTH, IMM_FILTER

% History:
%   01.11.2007 JH The first official version.
%
% Copyright (C) 2007 Jouni Hartikainen
%
% $Id: imm_update.m 111 2007-11-01 12:09:23Z jmjharti $
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

function [X_i,P_i,MU,X,P] = imm_update(X_p,P_p,c_j,ind,dims,Y,H,R)
    % Number of models
    m = length(X_p);

    % Space for update state mean, covariance and likelihood of measurements
    X_i = cell(1,m);
    P_i = cell(1,m);
    lambda = zeros(1,m);

    % Update for each model
    for i = 1:m
        % Update the state estimates
        [X_i{i}, P_i{i}, K, IM, IS, lambda(i)] = kf_update(X_p{i},P_p{i},Y,H{i},R{i});
    end

    % Calculate the model probabilities
    MU = zeros(1,m);
    c = sum(lambda.*c_j);
    MU = c_j.*lambda/c;

    % Output the combined updated state mean and covariance, if wanted.
    if nargout > 3
        % Space for estimates
        X = zeros(dims,1);
        P = zeros(dims,dims);
        % Updated state mean
        for i = 1:m
            X(ind{i}) = X(ind{i}) + MU(i)*X_i{i};
        end
        % Updated state covariance
        for i = 1:m
            P(ind{i},ind{i}) = P(ind{i},ind{i}) + MU(i)*(P_i{i} + (X_i{i}-X(ind{i}))*(X_i{i}-X(ind{i}))');
        end
    end


%% Adaptative Quadrature
% This is an adaptative quadrature algorithm for Simpson and Trapezoidal
% rule.

clear
clc
close all

%% Quadrature grid and initial values
% Integration range and values
%
a=0; % Lower limit
b=1; % Upper limit
n=10; % Initial number of grid divisions
h=(b-a)/n; % Initial step size
x=zeros(1,n+1); % Quadrature points vector
epsilon=1/(b-a)*(1e-8); % Decided error

%% Stablish quadrature point x values
x(1)=a;
for i=1:n
    x(i+1)=x(i)+h;
end

%% Run quadrature function
for i=1:n
    S(i)=SimpsonAQ(x(i),x(i+1),epsilon); %Preseted function of AQ
end
total=sum(S)


clear all;

fprintf('test Lasso weighted\n');
randn('seed',0);
% Data are generated
X=randn(64,10000);
X=X./repmat(sqrt(sum(X.^2)),[size(X,1) 1]);
D=randn(64,256);
D=D./repmat(sqrt(sum(D.^2)),[size(D,1) 1]);

% parameter of the optimization procedure are chosen
param.L=20; % not more than 20 non-zeros coefficients (default: min(size(D,1),size(D,2)))
param.lambda=0.15; % not more than 20 non-zeros coefficients
param.numThreads=8; % number of processors/cores to use; the default choice is -1
                    % and uses all the cores of the machine
param.mode=2;       % penalized formulation

W=rand(size(D,2),size(X,2));

tic
alpha=mexLassoWeighted(X,D,W,param);
t=toc;
toc

fprintf('%f signals processed per second\n',size(X,2)/t);


% ASTROTIK by Francesco Santilli
% R2BP (Restricted Two Bodies Problem)
% Compute the Minimum Orbit Intersection Distance (MOID)
%
% Usage: [d,f] = moid(orbitA,orbitB)
%
% where: orbit = [p e i o w] = 3d orbit elements
%        orbit = [p e w] = 2d orbit elements
%           p = semi-latus rectum [L] (p>0)
%           e = eccentricity [-] (0<=e<1)
%           i = inclinaion [rad]
%           o = raan [rad]
%           w = argument of perifocus [rad]
%        d = MOID [L]
%        f = [fA fB] = true anomalies [rad]

function [d,f] = moid(orbitA,orbitB)

    if ~(nargin == 2)
        error('Wrong number of input arguments.')
    end

    DA = check(orbitA,1);
    DB = check(orbitB,1);

    if DA ~= DB
        error('Wrong size of input arguments.')
    end

    if ~(DA==3 || DA==5)
        error('Wrong size of input arguments.')
    end
    d3 = (DA==5);

    % rotation matrix
    if d3
        [pA,eA,iA,oA,wA] = take(orbitA);
        [pB,eB,iB,oB,wB] = take(orbitB);
        RA = rotation([oA iA wA]);
        RB = rotation([oB iB wB]);
        RA = RA(1:2,:);
        RB = RB(1:2,:);
    else
        [pA,eA,wA] = take(orbitA);
        [pB,eB,wB] = take(orbitB);
        cwA = cos(wA);
        swA = sin(wA);
        cwB = cos(wB);
        swB = sin(wB);
        RA = [ cwA swA
             -swA cwA];
        RB = [ cwB swB
              -swB cwB];
    end

    if pA<=0 || pB<=0
        error('p must be a stricly positive value.')
    end

    if eA<0 || eB<0 || eA>=1 || eB>=1
        error('e must be in the range [0,1).')
    end

    L2 = pA^2+pB^2;
    f0 = [+1 +1 -1 -1
          +1 -1 +1 -1]*pi/2;

    f = zeros(2,4);
    d = zeros(1,4);
    ef = zeros(1,4);

    opt = optimset('LargeScale','on','Display','off','GradObj','on',...
                   'TolX',eps,'TolFun',0,... % 'Hessian','on',
                   'MaxFunEvals',Inf,'MaxIter',Inf);
    for k = 1:4
        try
            [f(:,k),d(k),ef(k)] = fminunc(@fun,f0(:,k),opt);
        catch
            f(:,k) = [NaN NaN]';
            d(k) = NaN;
            ef(k) = 0;
        end
    end

    bad = (ef==0);
    f(:,bad) = [];
    d(:,bad) = [];
    if isempty(f)
        error('No solution found.')
    end

    d = sqrt(d*L2);
    f = mod(f+pi,2*pi)-pi;
    [d,k] = min(d);
    f = f(:,k(1))';

    function [F,FF,FFF] = fun(f)

        fA = f(1);
        fB = f(2);

        cA = cos(fA);
        cB = cos(fB);
        sA = sin(fA);
        sB = sin(fB);

        rA = pA/(1+eA*cA);
        rB = pB/(1+eB*cB);

        A = rA*[cA sA]*RA;
        B = rB*[cB sB]*RB;

        D = A-B;
        F = D*D' / L2;

        if nargout > 1

            kA = rA^2/pA;
            kB = rB^2/pB;

            GA = [-sA eA+cA]*RA;
            GB = [-sB eB+cB]*RB;

            FA =  2*kA*D*GA';
            FB = -2*kB*D*GB';
            FF = [FA; FB] / L2;

            if nargout > 2

                kAA = 2*kA^2/rA*eA*sA;
                kBB = 2*kB^2/rB*eB*sB;

                GAA = -[cA sA]*RA;
                GBB = -[cB sB]*RB;

                FAA =  2*(kAA*D*GA' + kA*D*GAA' + kA^2*GA*GA');
                FBB = -2*(kBB*D*GB' + kB*D*GBB' + kB^2*GB*GB');
                FAB = -2*kA*kB*GA*GB';

                FFF = [FAA FAB
                       FAB FBB] / L2;

            end
        end

        %fprintf('%.20f %.20f -> %.20f\n',f',F);

    end

end


% Copyright Andrew Binning 2013
% Please feel free to use and modify this code as you see if fit. If you
% use this code in any academic work, please cite
% Andrew Binning, 2013.
% "Underidentified SVAR models: A framework for combining short and long-run restrictions with sign-restrictions,"
% Working Paper 2013/14, Norges Bank.
function C = generateDraw(C,k)
%==========================================================================
% Generates a draw that is consistent with the shock variance/covariance
% matrix. Based on Juan F. Rubio-Ramirez & Daniel F. Waggoner & Tao Zha, 2010.
% "Structural Vector Autoregressions: Theory of Identification and
% Algorithms for Inference," Review of Economic Studies, Oxford University Press, vol. 77(2), pages 665-696.
%
% inputs:
% C = initial impact matrix, usually from the cholesky decomposition of the
% forecast error variance decomposition
% k = number of dependent variables
%
% outputs:
% C = new draw of the short run impact matrix
%==========================================================================

newmatrix = randn(k,k);

[Q,R] = qr(newmatrix);

for ii = 1:k
    if R(ii,ii)<0
        Q(:,ii) = -Q(:,ii);
    end
end

C = C*Q;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculating average precision using 11 point
% averaging.
%
% We first linearly interpolate the PR curves,
% which turns out to be more robust to the
% number of points sampled on the PR cureve.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function ap = calcAP(rec, prec, interval)
if nargin < 3
    interval = 0:0.1:1;
end

% linear interpolation
[rec,ii] = sort(rec);
prec = prec(ii);
[rec,ii] = unique(rec);
prec = prec(ii);
Rq = 0:0.01:1;
Pq = interp1(rec,prec,Rq);
Pq(isnan(Pq)) = 0;
prec = Pq;
rec = Rq;

ap=0;
for t=interval
    p=max(prec(rec>=t));
    if isempty(p)
        p=0;
    end
    ap=ap+p/numel(interval);
end


% Log-harmonic scale
%
% Inputs
%  Hb    : Below this harmonic limit, the scale is linear
%          (similar to the mel scale which is linear below 1000Hz)
%          (e.g. 12)
%          Based on observation of the LF model, the asymptotic behavior of the
%          spectrum starts around the 12th harmonic (for the most tense voice)
%  Hmax  : The maxmimum number of harmonic considered during synthesis
%          (e.g. 256)
%  order : The reduced number of phase coefficients (e.g. 24)
%
% Outputs
%  hsl   : The log-harmonic scale
%
% Copyright (c) 2013 University of Crete - Computer Science Department(UOC-CSD)/
%                    Foundation for Research and Technology-Hellas - Institute
%                    of Computer Science (FORTH-ICS)
%
% License
%  This file is under the LGPL license,  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. This file 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.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%  Jonas Ballani for the bezier curve (see nested function)
%

function hsl = hlin2hlog(Hb, Hmax, order)

    %   The last compressed coef (i.e. order) corresponds to a given Hmax

    % Use linear scale below Hb (higher coefs will be overwritten)
    hsl = 1:Hmax;

    % Build a Bezier curve to start with a linear scale and finish smoothly
    % at (Hmax,order)
    p(1,:) = [Hb, Hb];
    p(2,:) = [order, order];
    p(3,:) = [Hmax, order];
    t = 0:0.01:1;
    [X,Y,p_bez] = CASTELJAU(0,1,p,t);
    hsl(Hb+1:end) = interp1(p_bez(:,1), p_bez(:,2), (Hb+1):Hmax);

    if 0
        hs = 1:Hmax;
        plot(hs, hsl, 'k');

        hold on;
        keyboard
    end

return

function [X,Y,val] = CASTELJAU(a,b,p,y)

    % function val = CASTELJAU(a,b,p,y)
    %
    % INPUT:  a   Linke Intervallgrenze
    %         b   Rechte Intervallgrenze
    %         p   Stützstellen (nx2-Matrix)
    %         y   Auswertungspunkte (Spaltenvektor)
    %
    % OUTPUT: val   Werte des Bezierpolynoms an y (mx2-Matrix)
    %
    % Date:   2007-11-05
    % Author: Jonas Ballani

    % Notes from degottex@csd.uoc.gr:
    %  From bezier.zip
    %  From http://m2matlabdb.ma.tum.de/download.jsp?MC_ID=7&SC_ID=8&MP_ID=480
    %  No license or copyright specified.

    n = size(p,1);
    m = length(y);
    T = zeros(n,n);
    val = zeros(m,2);
    X(:,1) = p(:,1);
    Y(:,1) = p(:,2);

    for j = 1:m
        for i = 2:n
            X(i:n,i) = (b-y(j))/(b-a)*X(i-1:n-1,i-1) + (y(j)-a)/(b-a)*X(i:n,i-1);
            Y(i:n,i) = (b-y(j))/(b-a)*Y(i-1:n-1,i-1) + (y(j)-a)/(b-a)*Y(i:n,i-1);
        end
        val(j,1) = X(n,n);
        val(j,2) = Y(n,n);
    end

return


function [route] = astar3(exbigraph,exbiloc,startnode,endnode,ts)
%% Written by Muhammet Balcilar, France
% all rights reverved

graph=exbigraph;
Loc=exbiloc;
n=size(graph,1);

curr=startnode;

Cost=[curr 0];
Hcost= sum(sum(abs(Loc(curr,:)-Loc(endnode,:))));

Route{1}=[curr];

Tabu=[curr];
cRoute=[curr];
cr=1;

i=1;
% if the endnode is found or all possibility is finished
while Hcost(cr)>0 %      sum(curr~=endnode)>0
    i=i+1;
    iter=i

    % find all possibility from current node
    %PosMove2=findpossmove(graph,curr,ts);
    PosMove=findpossmoveRec(graph,curr,ts);

    % find all possibility's cost from current to concerned node
    for j=1:size(PosMove,1)
        % manhattan distance for current and possible locs
        cost(j,1)=sum(sum( abs(Loc(curr,:)-Loc(PosMove(j,:),:))))+findindex(Cost,curr);
        % add time cost every step cost is just 1 move
        cost(j,1)=cost(j,1)+1;
        [val ind]=findindex(Cost,PosMove(j,:));
        if cost(j,1)<val
            Cost(ind,:)=[PosMove(j,:) cost(j,1)];
            Route{ind}=[cRoute; PosMove(j,:)];
            Hcost(ind,1)=sum(sum(abs(Loc(PosMove(j,:),:)-Loc(endnode,:))));
        end

    end

    % find minimum of normal cost plus heuristic cost of possible node
    tmp=Cost;
    tmp(:,end)=tmp(:,end)+Hcost;
    [mn cr]=finminexceptTabu(tmp,Tabu);

    if isinf(mn)
        break
    end
    % set minimum total cost's node as current node
    curr=Cost(cr,1:end-1);
    cRoute=Route{cr};
    % add current node to tabu list to prevent loops
    Tabu=[Tabu; curr];
end
% if current node is equal endnode means route is found
if Hcost(cr)==0
    route=cRoute;
else % i not means there is no way
    route=[];
end


function [ output ] = FW_T( par)

% this function implements Frank-Wolfe-thresholding method
% 0.5*norm(Omega.*(L+S-M), 'fro')^2 + lambda_1 ||L||_* + lambda_2 ||S||_1
%
%
%%  Cun Mu and John Wright, Mar '14

M = par.M; % data matrix
[m,n] = size(M);

epsilon = 10^-3; compare = 0; % by default (if no specification)
if isfield(par, 'epsilon') epsilon = par.epsilon; end
lambda_1 = par.lambda_1;
lambda_2 = par.lambda_2;
iter = par.iter;
display = par.display;
method = par.method; % power or exact
Omega = par.Omega;
rho = sum(sum(Omega))/m/n;
%% FW-T method

% initialization
L = zeros(m,n); S = zeros(m,n);
t_1 = 0; t_2 = 0;

U_1 = 0.5*norm(M,'fro')^2/lambda_1; % initial rough guess
U_2 = 0.5*norm(M,'fro')^2/lambda_2; % initial rough guess

history = 0; % to store the values of each iteration
tic
%% full observation scenario
%if ~isfield(par, 'Omega')
if rho == 1
    fprintf('full observation. \n');
    history(1) = norm(M,'fro')^2/2;
    temp = L + S - M;  % gradient
    count = 0;
    for k = 1: iter

        if k>=2
            if abs(history(k)-history(k-1))/history(k-1)<epsilon
                count = count+1;
            else
                count=0;
            end
            if count==5
                break;
                fprintf('--------------------------------\n')
                fprintf('total # of iter. used is %d. \n', k);
            end
        end

        fprintf('the current function value is %d \n', history(k));

        if mod(k,1) == 0
            fprintf('--------------------------------\n')
            fprintf('this is the %d th iter. \n', k);
        end

        %------------------linearization subproblem-----------------------%

        if  strcmp(method,'power') % approximate
            fprintf('power method is leveraged \n')
            [U ev V] =  power_method(temp, 5);
        else % exact
            [U,ev,V] = lansvd(temp,1,'l'); % top eigen
        end

        D_L = -U*V';

        if lambda_1 >= ev
            V_L = 0; V_t_1 = 0;
        else
            V_L = U_1*D_L; V_t_1 = U_1;
        end

        [mag ind] = max(vec(abs(temp)));
        j = floor((ind-1)/m)+1; i = mod(ind-1,m)+1;
        sign_ = sign(temp(i,j));
        D_S = zeros(m,n);
        D_S(i,j) = -sign_;

        if lambda_2 >= mag
            V_S = 0; V_t_2 = 0;
        else
            V_S = U_2*D_S; V_t_2 = U_2;
        end

        H = zeros(2,2);
        temp_1 = V_L-L; temp_2 = V_S-S; % temp = L + S - M;
        H(1,1) = norm(temp_1, 'fro')^2;
        H(2,2) = norm(temp_2, 'fro')^2;
        H(1,2) = sum(sum(temp_1.*temp_2));
        H(2,1) = H(1,2);
        f = zeros(1,2);
        f(1) = sum(sum(temp_1.*temp));
        f(2) = sum(sum(temp_2.*temp));
        f = f + [lambda_1*(V_t_1-t_1), lambda_2*(V_t_2-t_2)];

        % using QP solvers
        lb = zeros(2,1);
        ub = [1;1];
        options.Display = 'off';
        options.TolFun = 10^-5;
        x = quadprog(H,f,[],[],[],[],lb,ub,[],options);
        x = quadprog(H,f,[],[],[],[],lb,ub,[],options);

        %----------------------- update L and S --------------------------%
        alpha = x(1);
        beta = x(2);
        L = (1-alpha)*L + alpha*V_L; t_1 = t_1 + alpha*(V_t_1-t_1);
        S = (1-beta)*S + beta*V_S; t_2 = t_2 + beta*(V_t_2-t_2);

        %------------------------ thresholding----------------------------%

        temp_3 = M-L;

        S = max(temp_3 - lambda_2, 0);
        S = S + min(temp_3 + lambda_2, 0);

        t_2 = sum(sum(abs(S)));

        %----------- update U_1 and U_2 to a better esitmate -------------%
        temp = L + S - M;
        history(k+1) = norm(temp,'fro')^2/2+lambda_1*t_1+lambda_2*t_2;
        U_1 = min(history(k+1)/lambda_1,U_1);
        U_2 = min(history(k+1)/lambda_2,U_2);

    end

end

%% partial observation scenario
if rho<1
    fprintf('partial observation. \n');
    temp = Omega.*(L + S - M);  % gradient
    history(1) = norm(temp,'fro')^2/2+lambda_1*t_1+lambda_2*t_2;
    count = 0;

    for k = 1: iter

        if k>=2
            if abs(history(k)-history(k-1))*2/history(1)<epsilon
                count = count+1;
            else
                count=0;
            end
            if count==5
                break;
                fprintf('--------------------------------\n')
                fprintf('total # of iter. used is %d. \n', k);
            end
        end

        % current function value
        fprintf('the current function value is %d \n', history(k));
        fprintf('--------------------------------\n')
        fprintf('this is the %d th iter. \n', k);

        %------------------linearization subproblem-----------------------%

        if  strcmp(method,'power') % approximate
            [U ev V] =  power_method(temp, 5);
        else % exact
            [U,ev,V] = lansvd(temp,1,'l'); % top eigen
        end

        D_L = -U*V';

        if lambda_1 >= ev
            V_L = 0; V_t_1 = 0;
        else
            V_L = U_1*D_L; V_t_1 = U_1;
        end

        [mag ind] = max(vec(abs(temp)));
        j = floor((ind-1)/m)+1; i = mod(ind-1,m)+1;
        sign_ = sign(temp(i,j));
        D_S = zeros(m,n);
        D_S(i,j) = -sign_;

        if lambda_2 >= mag
            V_S = 0; V_t_2 = 0;
        else
            V_S = U_2*D_S; V_t_2 = U_2;
        end

        %--------------------- use QP (exact search) ---------------------%
        H = zeros(2,2);
        temp_1 = Omega.*(V_L-L); temp_2 = Omega.*(V_S-S); % temp = L + S - M;
        H(1,1) = norm(temp_1, 'fro')^2;
        H(2,2) = norm(temp_2, 'fro')^2;
        H(1,2) = sum(sum(temp_1.*temp_2));
        H(2,1) = H(1,2);
        f = zeros(1,2);
        f(1) = sum(sum(temp_1.*temp));
        f(2) = sum(sum(temp_2.*temp));
        f = f + [lambda_1*(V_t_1-t_1), lambda_2*(V_t_2-t_2)];

        % using QP solvers
        lb = zeros(2,1);
        ub = [1;1];
        options.Display = 'off';
        options.TolFun = 10^-5;
        x = quadprog(H,f,[],[],[],[],lb,ub,[],options);
        x = quadprog(H,f,[],[],[],[],lb,ub,[],options);

        %----------------------- update L and S --------------------------%
        alpha = x(1);
        beta = x(2);
        L = (1-alpha)*L + alpha*V_L; t_1 = t_1 + alpha*(V_t_1-t_1);
        S = (1-beta)*S + beta*V_S; t_2 = t_2 + beta*(V_t_2-t_2);

        %------------------------ thresholding----------------------------%
        temp_3 = S-Omega.*(L+S-M);;
        S = max(temp_3 - lambda_2, 0);
        S = S + min(temp_3 + lambda_2, 0);
        t_2 = norm(vec(S),1);

        %----------- update U_1 and U_2 to a better esitmate -------------%
        temp = Omega.*(L + S - M);  % gradient
        history(k+1) = norm(temp,'fro')^2/2+lambda_1*t_1+lambda_2*t_2;
        U_1 = min(history(k)/lambda_1,U_1);
        U_2 = min(history(k)/lambda_2,U_2);

    end

end
toc
%% output
output.L = L;
output.S = S;
output.hist = history;
output.iter = k;
end


function [T]=affineTransformationMatrixDirect(V1,V2)

%%
%Force input to 3D
if size(V1,2)==2
    V1(:,3)=0;
end

if size(V2,2)==2
    V2(:,3)=0;
end

%Expand to nx4
V1_M=V1;
V1_M(:,4)=1;
V2_M=V2;
V2_M(:,4)=1;

%Get transformation using left devide
T=(V1_M\V2_M)';

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function [layout]=getLayouts(vp,h,w,nsamp)
% Get candidate box layout. Samples mulitple rays from each vanishing point
%layout is given by corners of the walls.

imcor=[0 h+1;0 0;w+1 0;w+1 h+1];

[vp nothing]=ordervp(vp,h,w);
%inf conditions
infcond = vp(:,1)>50*w | vp(:,2)>50*h;

%inside image conds

inim = vp(:,1)>=1 & vp(:,1)<=w & vp(:,2)>=1 & vp(:,2)<=h;
rays=cell(1,3);

for vno=1:2
    rays{vno}=[];

    %general
    if ~inim(vno)
        ll1=[vp(vno,1) imcor(1,1) vp(vno,2) imcor(1,2)];
        ll2=[vp(vno,1) imcor(2,1) vp(vno,2) imcor(2,2)];
        ll3=[vp(vno,1) imcor(3,1) vp(vno,2) imcor(3,2)];
        ll4=[vp(vno,1) imcor(4,1) vp(vno,2) imcor(4,2)];
        lla=[ll1;ll1;ll1;ll2;ll2;ll3];
        llb=[ll2;ll3;ll4;ll3;ll4;ll4];

        veca = lla(:,[2,4])-lla(:,[1,3]);
        vecb = llb(:,[2,4])-llb(:,[1,3]);
        norma=(sum(veca.*veca,2)).^.5;
        normb=(sum(vecb.*vecb,2)).^.5;
        theta = acosd(dot(veca,vecb,2)./norma./normb);
        [vv ii]=max(theta);

        dtheta=theta(ii)/nsamp;
        rays{vno}(1,:)=lla(ii,:);% two extra rays passing outside the image
        rays{vno}(2,:)=llb(ii,:);

        veca=lla(ii,[2,4])-lla(ii,[1,3]);
        vecb=[w 0];
        norma=(sum(veca.*veca,2)).^.5;
        normb=(sum(vecb.*vecb,2)).^.5;
        ang = acosd(dot(veca,vecb,2)./norma./normb);

        veca=llb(ii,[2,4])-llb(ii,[1,3]);
        vecb=[w 0];
        norma=(sum(veca.*veca,2)).^.5;
        normb=(sum(vecb.*vecb,2)).^.5;
        ang = acosd(dot(veca,vecb,2)./norma./normb);

    else
        dtheta=180/nsamp;
    end

    for ang=dtheta:dtheta:180

        pa=[];
        pb=[];
        m=tand(ang);

        if isnan(m)
            pa=[vp(vno,1) h];
            pb=[vp(vno,1) 1];
        elseif m==0
            pa=[1 vp(vno,2)];
            pb=[w vp(vno,2)];
        else
            c=vp(vno,2)-m*vp(vno,1);
            x=(h-c)/m;
            p(1,:)=[x h];%intersection with last row of image
            x=(1-c)/m;
            p(2,:)=[x 1];%intersection with 1st row of image
            y=m*w+c;
            p(3,:)=[w y];
            y=m+c;
            p(4,:)=[1 y];

            ind=find(p(:,1) <= w & p(:,1)>=1 & p(:,2)<=h & p(:,2)>=1);
            if numel(ind)==2
                pa=p(ind(1),:);
                pb=p(ind(2),:);
            end
        end

        if numel(pa) ==2 &numel(pb)==2
            rays{vno}=[rays{vno};pa(1) pb(1) pa(2) pb(2)]; %lines=[x1 x2 y1 y2];

        end
    end
end

p1 = [rays{1}(:, [1 3]) ones(size(rays{1}, 1), 1)];
p2 = [rays{1}(:, [2 4]) ones(size(rays{1}, 1), 1)];

ll1 = cross(p1, p2);
ll1 = ll1 ./ repmat(sqrt(sum(ll1.^2,2)), 1, 3);
% ll=cross([vp(2,:) 1],[vp(3,:) 1]);
ll=cross([vp(2,:) 1],[vp(3,:) 1]);
ll = ll ./ repmat(sqrt(sum(ll.^2,2)), 1, 3);
aa = cross(ll1,repmat(ll,[size(ll1,1),1]));
aa=[aa(:,1)./aa(:,3) aa(:,2)./aa(:,3)];
inds = find(aa(:,1)<vp(3,1));
if numel(inds)>0
    rays_left = rays{1}(inds,:);
else
    rays_left = [vp(1,1) vp(3,1)-10 vp(1,2) vp(3,2)];
end
inds = find(aa(:,1)>=vp(3,1));
if numel(inds)>0
    rays_right = rays{1}(inds,:);
else
    rays_right = [vp(1,1) vp(3,1)+10 vp(1,2) vp(3,2)];
end

p1 = [rays{2}(:, [1 3]) ones(size(rays{2}, 1), 1)];
p2 = [rays{2}(:, [2 4]) ones(size(rays{2}, 1), 1)];

ll1 = cross(p1, p2);
ll1 = ll1 ./ repmat(sqrt(sum(ll1.^2,2)), 1, 3);
ll=cross([vp(1,:) 1],[vp(3,:) 1]);
ll = ll ./ repmat(sqrt(sum(ll.^2,2)), 1, 3);
aa = cross(ll1,repmat(ll,[size(ll1,1),1]));
aa=[aa(:,1)./aa(:,3) aa(:,2)./aa(:,3)];
inds = find(aa(:,2)<vp(3,2));
if numel(inds)>0
    rays_top = rays{2}(inds,:);
else
    rays_top = [vp(2,1) vp(3,1) vp(2,2) vp(3,2)-10];
end
inds = find(aa(:,2)>=vp(3,2));
if numel(inds)>0
    rays_bottom = rays{2}(inds,:);
else
    rays_bottom = [vp(2,1) vp(3,1) vp(2,2) vp(3,2)+10];
end

clear rays

[uu vv ww xx]=ndgrid(1:size(rays_left,1),...
    1:size(rays_right,1),...
    1:size(rays_bottom,1),...
    1:size(rays_top,1));
uu=uu(:);
vv=vv(:);
ww=ww(:);
xx=xx(:);

[xs,ys] = IntersectLines(rays_left(uu(:),:),rays_bottom(ww(:),:));
corners_x(:,1) = xs;
corners_y(:,1) = ys;

[xs,ys] = IntersectLines(rays_left(uu(:),:),rays_top(xx(:),:));
corners_x(:,2) = xs;
corners_y(:,2) = ys;

[xs,ys] = IntersectLines(rays_right(vv(:),:),rays_top(xx(:),:));
corners_x(:,3) = xs;
corners_y(:,3) = ys;
[xs,ys] = IntersectLines(rays_right(vv(:),:),rays_bottom(ww(:),:));
corners_x(:,4) = xs;
corners_y(:,4) = ys;

corners_x = round(corners_x);
corners_y = round(corners_y);

ind=find(corners_x >=1 & corners_x <=w & corners_y >=1 & corners_y <=h );
in_img=zeros(size(corners_x));
in_img(ind)=1;

corners_x_temp = corners_x.*in_img;
corners_y_temp = corners_y.*in_img;

[unq,I,J] = unique([corners_x_temp corners_y_temp],'rows');

corners_x = corners_x(I,:);
corners_y = corners_y(I,:);

layout = [];
for i=1:4
    layout = [layout corners_x(:,i) corners_y(:,i)];
end

return;


%  Figure 10.24      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
%   fig10_24.m is a script to generate Fig. 10.24,
%   the transient response of the LQR symmetric rootlocus compensator
%   of the satellite position control, non-colocated case WITH ESTIMATOR

% parameter values
m=[1, 0.1]; k=[0, 0.091] ; d=[0, 0.0036]; k1=[0, 0.4];
% call function
[f,g,h,j] = twomass(m,k,d);
s=[f, g;h, 0];r=[0*g;1]; n=s\r;nx=n(1:4);nu=n(5);
% call function
[f1,g,h,j] = twomass(m,k1,d);

% form G(s)G(-s) model
a=[f, 0*f;
-h'*h, -f'];
b=[g;0*g];
d=[0];
c=[0*h, g'];
hold off; clf
P=eig(a-b*c*0.1621);
pc=P(real(P<0)==1);
K=place(f,g,pc);
nbar=nu+K*nx;
% eig(f-g*K)
P=eig(a-b*c*3.056e7);
pe=P(real(P<0)==1);
L=place(f',h',pe)';
ac=f-g*K-L*h ;bc=L;cc=K;dc=0;
[Aol,Bol,Col,Dol]=series(ac,bc,cc,dc,f,g,h,j);
[acl,bcl,ccl,dcl]=feedback(f,g,h,j,ac,bc,cc,dc);
[acl1,bcl,ccl,dcl]=feedback(f1,g,h,j,ac,bc,cc,dc);
bcl= nbar*[g;g];
t=0:.25:30;
syscl=ss(acl,bcl,ccl,dcl);
step(syscl,t);
hold on;
grid;
gtext('nominal case')
syscl1=ss(acl1,bcl,ccl,dcl);
step(syscl1,t) ;
gtext('stiff-spring case');
title('Fig. 10.24 Closed-loop step response for the SRL design with an estimator')


% Calculate the value of lambda so that if lambda >= lambdamax, the TVD
% functional solved by l1pwc is minimized by the trivial constant
% solution x = mean(y). This can then be used to determine a useful range
% of values of lambda, for example.
%
% Usage:
% lambdamax = l1pwclmax(y)
%
% Input arguments:
% - y          Original signal to denoise, size N x 1.
%
% Output arguments:
% - lambdamax  Value of at which x = mean(y) is the output of the l1pwc
%              function.
%
% (c) Max Little, 2010. If you use this code for your research, please
% cite:
% M.A. Little, Nick S. Jones (2010)
% "Sparse Bayesian Step-Filtering for High-Throughput Analysis of Molecular
% Machine Dynamics", in 2010 IEEE International Conference on Acoustics,
% Speech and Signal Processing, 2010, ICASSP 2010 Proceedings.
%

function lambdamax = ML_l1pwclmax(y)

narginchk(1,1);
y = y(:);
N = length(y);
M = N - 1;

% Construct sparse operator matrices
I1 = speye(M,M);
O1 = spalloc(M,1,M);
D = [I1 O1]-[O1 I1];

DDT = D*D';
Dy  = D*y;

lambdamax = max(abs(DDT\Dy));

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  MATLAB Code for                                                  %
%                                                                   %
%  Multi-Objective Particle Swarm Optimization (MOPSO)              %
%  Version 1.0 - Feb. 2011                                          %
%                                                                   %
%  According to:                                                    %
%  Carlos A. Coello Coello et al.,                                  %
%  "Handling Multiple Objectives with Particle Swarm Optimization," %
%  IEEE Transactions on Evolutionary Computation, Vol. 8, No. 3,    %
%  pp. 256-279, June 2004.                                          %
%                                                                   %
%  Developed Using MATLAB R2009b (Version 7.9)                      %
%                                                                   %
%  Programmed By: S. Mostapha Kalami Heris                          %
%                                                                   %
%         e-Mail: sm.kalami@gmail.com                               %
%                 kalami@ee.kntu.ac.ir                              %
%                                                                   %
%       Homepage: http://www.kalami.ir                              %
%                                                                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function G=CreateHypercubes(costs,ngrid,alpha)

    nobj=size(costs,1);

    empty_grid.Lower=[];
    empty_grid.Upper=[];
    G=repmat(empty_grid,nobj,1);

    for j=1:nobj

        min_cj=min(costs(j,:));
        max_cj=max(costs(j,:));

        dcj=alpha*(max_cj-min_cj);

        min_cj=min_cj-dcj;
        max_cj=max_cj+dcj;

        gx=linspace(min_cj,max_cj,ngrid-1);

        G(j).Lower=[-inf gx];
        G(j).Upper=[gx inf];

    end

end


function [w0] = mci_interp_init (Y,M)
% Linear interpolate to t=0
% FORMAT [w0] = mci_interp_init (Y,M)
%
% Y     Cell of data from multiple subjects
%       Y{n}.y, Y{n}.ind for n=1..N
% M     Model structure
%
% w0    [d x N] matrix of initial states
%       where d is number of states
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: mci_interp_init.m 6548 2015-09-11 12:39:47Z will $

N=length(Y);
d=size(Y{1}.y,2);
doplot=0;

for n=1:N,
    for j=1:d,

        % Fit
        Nt=size(Y{n}.y,1);
        xd=[Y{n}.ind(:),ones(Nt,1)];
        yd=Y{n}.y(:,j);
        beta=pinv(xd)*yd;

        % Extrapolate
        xt=[[1:M.N]',ones(M.N,1)];
        yhat=xt*beta;

        if doplot
            figure;plot(Y{n}.ind,yd,'x');
            hold on; plot([1:M.N]',yhat,'r');
        end
        w0(j,n)=yhat(1);
    end
end


function imgOut = ConvertXYZtoYxy(img, inverse)
%
%       imgOut = ConvertxXYZtoYxy(img, inverse)
%
%
%        Input:
%           -img: image to convert from XYZ to Yxy or from Yxy to XYZ.
%           -inverse: takes as values 0 or 1. If it is set to 0 the
%                     transformation from XYZ to Yxy is applied, otherwise
%                     the transformation from Yxy to XYZ.
%
%        Output:
%           -imgOut: converted image in Yxy or XYZ.
%
%     Copyright (C) 2013  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

check3Color(img);
[r, c, col] = size(img);
imgOut = zeros(r, c, col);

if(inverse == 0)%forward transform
    norm = zeros(r, c);
    for i=1:3
        norm = norm + img(:,:,i);
    end

    imgOut(:,:,1) = img(:,:,2);

    imgOut(:,:,2) = img(:,:,1) ./ (norm);
    imgOut(:,:,3) = img(:,:,2) ./ (norm);
end

if(inverse == 1)%inverse transform
    Y_over_y = img(:,:,1) ./ img(:,:,3);
    imgOut(:,:,1) = Y_over_y .* img(:,:,2);
    imgOut(:,:,2) = img(:,:,1);
    imgOut(:,:,3) = Y_over_y .* (1.0 - img(:,:,2) - img(:,:,3));
end

imgOut = RemoveSpecials(imgOut);

end


%% The SantaFe example
%
% Simulate a set of pole figures for the SantaFe standard ODF, estimate
% an ODF and compare it to the inital SantaFe ODF.

%% Open in Editor
%

%% Simulate pole figures

CS = crystalSymmetry('m-3m');

% crystal directions
h = [Miller(1,0,0,CS),Miller(1,1,0,CS),Miller(1,1,1,CS),Miller(2,1,1,CS)];

% specimen directions
r = equispacedS2Grid('resolution',5*degree,'antipodal');

% pole figures
pf = calcPoleFigure(SantaFe,h,r);

% add some noise
pf = noisepf(pf,100);

% plot them
plot(pf,'MarkerSize',5)
mtexColorMap LaboTeX

%% ODF Estimation with Ghost Correction
rec = calcODF(pf)

%% ODF Estimation without Ghost Correction

rec2 = calcODF(pf,'NoGhostCorrection')

%% Error analysis

% calculate RP error
calcError(rec,SantaFe)

% difference plot between meassured and recalculated pole figures
plotDiff(pf,rec)

%% Plot estimated pole figures

plotPDF(rec,pf.h,'antipodal')

%% Plot estimated ODF (Ghost Corrected)

plot(rec,'sections',18,'resolution',5*degree,...
  'contourf','FontSize',10,'silent','figSize','large','minmax')
mtexColorMap white2black

%% Plot odf

plot(SantaFe,'sections',18,'contourf','FontSize',10,'silent',...
  'figSize','large','minmax')
mtexColorMap white2black

%% Plot Fourier Coefficients

%%
close all;
% true ODF
plotSpektra(SantaFe,'bandwidth',32,'linewidth',2)
% keep plot for adding the next plots
hold all

% With ghost correction:
plotSpektra(rec,'bandwidth',32,'linewidth',2)

% Without ghost correction:
plotSpektra(rec2,'bandwidth',32,'linewidth',2)

legend({'true ODF','with ghost correction','without ghost correction'})
% next plot command overwrites plot
hold off


function [im]=mrgb2gray(im,met)
% MRGB2GRAY - convert RGB images to grayscale
%
% im = mrgb2gray(M,met);
%
% M - image matrix or filename. The size of the image should be
% MxNx3 when it is loaded by imread.
%
% MET - method for conversion. Should be one of
%
%       'default'   - uses a weight of [0.3 0.59 0.11] for the R, G and
%                     B respectively
%       'max'       - uses the maximum value of the R, G or B for
%                     each pixel
%       'mean'      - uses the mean of the RGB for each pixel. This
%                     corresponds to using a weight of
%                     [0.33 0.33 0.33]
%       'median'    - as above except it uses the median value
%       'min'       - as above except it uses the minimum value
%       'desat'     - computes (max(R,G,B)+min(R,G,B))/2 for each
%                     pixel. This produces a lower contrast image.
%
% MET can also be a 1x3 array with weights.
%
% examples:
%           im=mrgb2gray(im,'max');
%           im=mrgb2gray(im);
%           im=mrgb2gray(im,[0.3 0.5 0.2]);
%           im=mrgb2gray('filename.jpg');
%           im=mrgb2gray('filename.jpg',[0.25 0.60 0.15]);
%           im=mrgb2gray('filename.jpg','desat');
%
% See also: rgb2gray, isrgb, misrgb
%

% version 0.22 - bug fix: under 'mean' changed occurence of 'min' to 'mean'
% version 0.21 - cleanup of code
% 24.08.2006
%
% version 0.2 - first released version
% J.K.Sveen@damtp.cam.ac.uk, 2004, August 27.
% Distributed under the terms of the GNU GPL:
% http://www.gnu.org/copyleft/gpl.html

if nargin==1
  met='default';
end

if ischar(im)
  im=imread(im);
end

[sx,sy,sz]=size(im);

if sz~=3
  disp('Error. This image is not RGB. Aborting.'); return
end

wasuint=isa(im,'uint8');
im=double(im);
if ~ischar(met)
  T=met(:);
  im=reshape(im,sx*sy,sz); %put image in an M*N by 3 array
  im=im*T; % multiply the weights
  im=reshape(im,sx,sy); % reshape the image back to correct size
else
  switch lower(met)
   case {'default','gimp','standard'}

    T=[0.3 0.59 0.11]'; % weights for the individual colors. These are
			% apparently the same weights as used in GIMP:
			%
			% http://gimp-savvy.com/BOOK/index.html?node54.html

    im=reshape(im,sx*sy,sz); %put image in an M*N by 3 array
    im=im*T; % multiply the weights, 0.3*R, 0.59*G, 0.11*B
    im=reshape(im,sx,sy); % reshape the image back to correct size
   case {'max','Max'}
    im=reshape(im,sx*sy,sz); %put image in an M*N by 3 array
    im=max(im,[],2); %rotate image and take mean of all columns, then
                  %rotate back. We now have an M*N by 1 array
    im=reshape(im,sx,sy); % reshape the image back to size M by N
   case {'mean','average'}
    im=reshape(im,sx*sy,sz); %put image in an M*N by 3 array
    im=mean(im,2);
    im=reshape(im,sx,sy); % reshape the image back to correct size
   case {'median'}
    im=reshape(im,sx*sy,sz); %put image in an M*N by 3 array
    im=median(im,2);
    im=reshape(im,sx,sy); % reshape the image back to correct size
   case {'min'}
    im=reshape(im,sx*sy,sz); %put image in an M*N by 3 array
    im=min(im,[],2);
    im=reshape(im,sx,sy); % reshape the image back to correct size
   case {'desat','desaturate'}
    im=reshape(im,sx*sy,sz); %put image in an M*N by 3 array
    im=(max(im,[],2) + min(im,[],2))/2;
    im=reshape(im,sx,sy); % reshape the image back to correct size
   otherwise
    disp('Unknown method. Check your input.'); return
  end
end

if wasuint % change back to uint8 if that was the input
  im=uint8(im);
end


function airy_ai_int_values_test ( )

%*****************************************************************************80
%
%% AIRY_AI_INT_VALUES_TEST demonstrates the use of AIRY_AI_INT_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'AIRY_AI_INT_VALUES_TEST:\n' );
  fprintf ( 1, '  AIRY_AI_INT_VALUES stores values of \n' );
  fprintf ( 1, '  the integral of the Airy Ai function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X           FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = airy_ai_int_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


function varargout = drawEllipseAxes(varargin)
%DRAWELLIPSEAXES Draw the main axes of an ellipse as line segments.
%
%   drawEllipseAxes(ELLI)
%   drawEllipseAxes(..., STYLE)
%   drawEllipseAxes(..., NAME, VALUE)
%   Draw the axes of the ellipse given by ELLI onto the currrent axis.
%   STYLE specifies the drawing style using a short character array like
%   'b', 'k:', 'm-'...
%   More complex drawing style can be specified using plot-like parameter
%   name-value pairs.
%
%   drawEllipseAxes(AX, ELLI)
%   Specifies the axes to draw the ellipse on.
%
%   Example
%     elli = [50 50  40 20  30];
%     figure; hold on; axis equal; axis([0 100 0 100]);
%     drawEllipse(elli, 'LineWidth', 2, 'Color', 'b')
%     drawEllipseAxes(elli, 'k')
%
%   See also
%     ellipses2d, drawEllipse, drawEllipseArc
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2022-09-11, using Matlab 9.9.0.1570001 (R2020b) Update 4
% Copyright 2022 INRAE - BIA Research Unit - BIBS Platform (Nantes)

%% Extract input arguments

% extract handle of axis to draw on
if isAxisHandle(varargin{1})
    ax = varargin{1};
    varargin(1) = [];
else
    ax = gca;
end

% extract dawing style strings
styles = {};
for iElli = 1:length(varargin)
    if ischar(varargin{iElli})
        styles = varargin(iElli:end);
        varargin(iElli:end) = [];
        break;
    end
end

% retrieve ellipse parameters
ellipse = varargin{1};
x0 = ellipse(:, 1);
y0 = ellipse(:, 2);
a  = ellipse(:, 3);
b  = ellipse(:, 4);
theta = ellipse(:, 5);
nElli = length(x0);

%% Process drawing of a set of ellipses

% angular positions of edge extremities
ti = [0 pi  pi/2 3*pi/2];

% compute position of points to draw each ellipse
h = zeros(2 * nElli , 1);
for iElli = 1:nElli
    % pre-compute rotation angles (given in degrees)
    cot = cosd(theta(iElli));
    sit = sind(theta(iElli));

    % compute position of points used to draw current ellipse
    xt = x0(iElli) + a(iElli) * cos(ti) * cot - b(iElli) * sin(ti) * sit;
    yt = y0(iElli) + a(iElli) * cos(ti) * sit + b(iElli) * sin(ti) * cot;

    % stores handle to graphic object
    h(2 * iElli - 1) = plot(ax, xt(1:2), yt(1:2), styles{:});
    h(2 * iElli)     = plot(ax, xt(3:4), yt(3:4), styles{:});
end

% return handles if required
if nargout > 0
    varargout = {h};
end


function [W] = TW2W(TW)
% Convert power from terawatts to watts.
% Chad A. Greene 2012
W = TW*1e+12;


function quadmom_test ( )

%*****************************************************************************80
%
%% QUADMOM_TEST tests the QUADMOM library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 October 2013
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'QUADMOM_TEST\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the QUADMOM library.\n' );

  quadmom_test01 ( );
  quadmom_test02 ( );
  quadmom_test03 ( );
  quadmom_test04 ( );
  quadmom_test05 ( );
  quadmom_test06 ( );
  quadmom_test07 ( );
  quadmom_test08 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'QUADMOM_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

 return
end


function [Va, success] = dcpf(B, Pbus, Va0, ref, pv, pq)
%DCPF  Solves a DC power flow.
%   [VA, SUCCESS] = DCPF(B, PBUS, VA0, REF, PV, PQ) solves for the bus
%   voltage angles at all but the reference bus, given the full system
%   B matrix and the vector of bus real power injections, the initial
%   vector of bus voltage angles (in radians), and column vectors with
%   the lists of bus indices for the swing bus, PV buses, and PQ buses,
%   respectively. Returns a vector of bus voltage angles in radians.
%
%   See also RUNDCPF, RUNPF.

%   MATPOWER
%   Copyright (c) 1996-2016, Power Systems Engineering Research Center (PSERC)
%   by Carlos E. Murillo-Sanchez, PSERC Cornell & Universidad Nacional de Colombia
%   and Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% constant
Va_threshold = 1e5;     %% arbitrary threshold on |Va| for declaring failure

%% initialize result vector
Va = Va0;
success = 1;    %% successful by default

%% set up to trap non-singular matrix warnings
[lastmsg, lastid] = lastwarn;
lastwarn('');

%% update angles for non-reference buses
Va([pv; pq]) = B([pv; pq], [pv; pq]) \ ...
                (Pbus([pv; pq]) - B([pv; pq], ref) * Va0(ref));

[msg, id] = lastwarn;
%% Octave is not consistent in assigning proper warning id, so we'll just
%% check for presence of *any* warning
if ~isempty(msg) || max(abs(Va)) > Va_threshold
    success = 0;
end

%% restore warning state
lastwarn(lastmsg, lastid);


% op_complexConj.m
% Jamie Near, McGill University 2014.
%
% USAGE:
% out=op_complexConj(in)
%
% DESCRIPTION:
% take the complex conjugate of the data;
%
% INPUTS:
% in	= Input data in matlab structure format.
%
% OUTPUTS:
% out   = Output following conjugation.

function out=op_complexConj(in);

fids=in.fids;
sz=size(fids);

fids=conj(fids);

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

%Calculate t and ppm arrays using the calculated parameters:
f=[(-in.spectralwidth/2)+(in.spectralwidth/(2*sz(1))):in.spectralwidth/(sz(1)):(in.spectralwidth/2)-(in.spectralwidth/(2*sz(1)))];
%ppm=-f/(in.Bo*42.577);
% ppm=-f/(3*42.577);
ppm = -f/(in.Bo*in.gamma);
if strcmp(in.nucleus,'1H')
    ppm=ppm+4.65;
end

%t=[0:in.dwelltime:(sz(1)-1)*in.dwelltime];
t=[in.dwelltime:in.dwelltime:sz(1)*in.dwelltime];

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

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


function [predictedCluster,centroids,eigens] = kernelkmeans(Kn, k)

    [H, ~] = eigs(Kn, k);
    H_normalized = H ./ repmat(sqrt(sum(H.^2, 2)), 1, k);
    predictedCluster = kmeans(H_normalized, k);

    eigens = H;
    centroids = zeros(k,k);

    for i = 1:k
        centroids(i,:) = sum(H_normalized(predictedCluster==i,:),1)/size(H_normalized(predictedCluster==i,:),1);
    end
end


classdef AvoidObstacles < simiam.controller.Controller

% Copyright (C) 2013, Georgia Tech Research Corporation
% see the LICENSE file included with this software

    properties

        % memory banks
        E_k
        e_k_1

        % gains
        Kp
        Ki
        Kd

        % plot support
        p

        % sensor geometry
        calibrated
        sensor_placement
    end

    properties (Constant)
        inputs = struct('v', 0);
        outputs = struct('v', 0, 'w', 0)
    end

    methods

        function obj = AvoidObstacles()
            obj = obj@simiam.controller.Controller('avoid_obstacles');
            obj.calibrated = false;

            obj.Kp = 5;
            obj.Ki = 0.01;
            obj.Kd = 0.01;

            obj.E_k = 0;
            obj.e_k_1 = 0;
        end

        function outputs = execute(obj, robot, state_estimate, inputs, dt)

            % Compute the placement of the sensors
            if(~obj.calibrated)
                obj.set_sensor_geometry(robot);
            end

            % Unpack state estimate
            [x, y, theta] = state_estimate.unpack();

            % Poll the current IR sensor values 1-5
            ir_distances = robot.get_ir_distances();

            % Interpret the IR sensor measurements geometrically
            ir_distances_wf = obj.apply_sensor_geometry(ir_distances, state_estimate);

            % 1. Compute the heading vector for obstacle avoidance

            sensor_gains = [1 1 0.5 1 1];
            u_i = (ir_distances_wf-repmat([x;y],1,5))*diag(sensor_gains);
            u_ao = sum(u_i,2);

            % 2. Compute the heading and error for the PID controller
            theta_ao = atan2(u_ao(2),u_ao(1));
            e_k = theta_ao-theta;
            e_k = atan2(sin(e_k),cos(e_k));

            e_P = e_k;
            e_I = obj.E_k + e_k*dt;
            e_D = (e_k-obj.e_k_1)/dt;

            % PID control on w
            v = inputs.v;
            w = obj.Kp*e_P + obj.Ki*e_I + obj.Kd*e_D;

            % Save errors for next time step
            obj.E_k = e_I;
            obj.e_k_1 = e_k;

            % plot
            obj.p.plot_2d_ref(dt, theta, theta_ao, 'g');

%             fprintf('(v,w) = (%0.4g,%0.4g)\n', v,w);

            outputs.v = v;
            outputs.w = w;
        end

        % Helper functions

        function ir_distances_wf = apply_sensor_geometry(obj, ir_distances, state_estimate)

            % 1. Apply the transformation to robot frame.

            ir_distances_rf = zeros(3,5);
            for i=1:5
                x_s = obj.sensor_placement(1,i);
                y_s = obj.sensor_placement(2,i);
                theta_s = obj.sensor_placement(3,i);

                R = obj.get_transformation_matrix(x_s,y_s,theta_s);
                ir_distances_rf(:,i) = R*[ir_distances(i); 0; 1];
            end

            % 2. Apply the transformation to world frame.

            [x,y,theta] = state_estimate.unpack();

            R = obj.get_transformation_matrix(x,y,theta);
            ir_distances_wf = R*ir_distances_rf;

            ir_distances_wf = ir_distances_wf(1:2,:);
        end

        function set_sensor_geometry(obj, robot)
            obj.sensor_placement = zeros(3,5);
            for i=1:5
                [x, y, theta] = robot.ir_array(i).location.unpack();
                obj.sensor_placement(:,i) = [x; y; theta];
            end
            obj.calibrated = true;
        end

        function R = get_transformation_matrix(obj, x, y, theta)
            R = [cos(theta) -sin(theta) x; sin(theta) cos(theta) y; 0 0 1];
        end

        function reset(obj)
            % Reset accumulated and previous error
            obj.E_k = 0;
            obj.e_k_1 = 0;
        end

    end

end


function [desired_state] = circle(t, qn)
% CIRCLE trajectory generator for a circle

% =================== Your code goes here ===================
% You have to set the pos, vel, acc, yaw and yawdot variables
time_tol = 12;
radius = 5;
dt = 0.0001;

    function pos = pos_from_angle(a)
        pos = [radius*cos(a); radius*sin(a); 2.5*a/(2*pi)];
    end

    function vel = get_vel(t)
        angle1 = tj_from_line(0, 2*pi, time_tol, t);
        pos1 = pos_from_angle(angle1);
        angle2 = tj_from_line(0, 2*pi, time_tol, t+dt);
        vel = (pos_from_angle(angle2) - pos1)/dt;
    end

if t > time_tol
    pos = [radius; 0; 2.5];
    vel = [0;0;0];
    acc = [0;0;0];
else
    angle = tj_from_line(0, 2*pi, time_tol, t);
    pos = pos_from_angle(angle);
    vel = get_vel(t);
    acc = (get_vel(t+dt) - get_vel(t))/dt;
end

yaw = 0;
yawdot = 0;
% =================== Your code ends here ===================

desired_state.pos = pos(:);
desired_state.vel = vel(:);
desired_state.acc = acc(:);
desired_state.yaw = yaw;
desired_state.yawdot = yawdot;

end


function [Detect] = cfar_ca1D_square(Xcube,noiseWin,guardLen,Pfa,wrapMode,ord_stat)
N = noiseWin*2;
alpha = N*(Pfa^(-1/N)-1);
alpha_oneside = noiseWin*(Pfa^(-1/noiseWin)-1);
Xcube = Xcube.^2;
Xlength = length(Xcube);
Detect = [];
numOfDet = 0;
% ord_stat = 0.7;
% if not CAOS-CFAR, set ord_stat = 1

if wrapMode == 0    %%% disabled warpped mode
    for i = 1:Xlength
        if i < noiseWin+guardLen+1  %%% one-sided comparision for left section
            Xcube_select = sort(Xcube(i+guardLen+1:i+guardLen+noiseWin), 'descend');
            num_filter = round(length(Xcube_select) * (1-ord_stat));
            noiseWin_len = noiseWin - num_filter;
            if num_filter > 0
                Xcube_select(1:num_filter) = 0;
            end
            noise_estimate = sum(Xcube_select)/noiseWin_len;
            if Xcube(i) > alpha_oneside*noise_estimate
                numOfDet = numOfDet + 1;
                Detect(1,numOfDet) = i; %%% index
                Detect(2,numOfDet) = Xcube(i);  %%% object power
                Detect(3,numOfDet) = noise_estimate;  %%% estimated noise
            end
        elseif i < Xlength-noiseWin-guardLen+1  %%% two-sided comparison for middle section
            Xcube_select = sort(Xcube(i+guardLen+1:i+guardLen+noiseWin), 'descend');
            Xcube_select2 = sort(Xcube(i-guardLen-noiseWin:i-guardLen-1), 'descend');
            num_filter = round(length(Xcube_select) * (1-ord_stat));
            noiseWin_len = noiseWin - num_filter;
            if num_filter > 0
                Xcube_select(1:num_filter) = 0;
                Xcube_select2(1:num_filter) = 0;
            end
            noise_estimate = (sum(Xcube_select) + sum(Xcube_select2))/(2*noiseWin_len);
            if Xcube(i) > alpha*noise_estimate
                numOfDet = numOfDet + 1;
                Detect(1,numOfDet) = i; %%% index
                Detect(2,numOfDet) = Xcube(i);  %%% object power
                Detect(3,numOfDet) = noise_estimate;  %%% estimated noise
            end
        else     %%%  one-sided comparision for right section
            Xcube_select = sort(Xcube(i-guardLen-noiseWin:i-guardLen-1), 'descend');
            num_filter = round(length(Xcube_select) * (1-ord_stat));
            noiseWin_len = noiseWin - num_filter;
            if num_filter > 0
                Xcube_select(1:num_filter) = 0;
            end
            noise_estimate = sum(Xcube_select)/noiseWin_len;
            if Xcube(i) > alpha_oneside*noise_estimate
                numOfDet = numOfDet + 1;
                Detect(1,numOfDet) = i; %%% index
                Detect(2,numOfDet) = Xcube(i);  %%% object power
                Detect(3,numOfDet) = noise_estimate;  %%% estimated noise
            end
        end
    end
else       %%% enabled wrapped mode
    for i = 1:Xlength
        if i < noiseWin+guardLen+1  %%% two-sided comparision for left section with wrap
            %%% discuss the wrap scenario
            if i <= guardLen
                noise_estimate = (sum(Xcube(i+guardLen+1:i+guardLen+noiseWin))...
                    + sum(Xcube(Xlength+i-guardLen-noiseWin:Xlength+i-guardLen-1)))/N;
            else
                noise_estimate = (sum(Xcube(i+guardLen+1:i+guardLen+noiseWin))...
                    + sum(Xcube(Xlength+i-guardLen-noiseWin:Xlength))+sum(Xcube(1:i-1-guardLen)))/N;
            end

            if Xcube(i) > alpha*noise_estimate
                numOfDet = numOfDet + 1;
                Detect(1,numOfDet) = i; %%% index
                Detect(2,numOfDet) = Xcube(i);  %%% object power
                Detect(3,numOfDet) = noise_estimate;  %%% estimated noise
            end

        elseif i < Xlength-noiseWin-guardLen+1  %%% two-sided comparison for middle section
            noise_estimate = (sum(Xcube(i+guardLen+1:i+guardLen+noiseWin))...
                + sum(Xcube(i-guardLen-noiseWin:i-guardLen-1)))/N;
            if Xcube(i) > alpha*noise_estimate
                numOfDet = numOfDet + 1;
                Detect(1,numOfDet) = i; %%% index
                Detect(2,numOfDet) = Xcube(i);  %%% object power
                Detect(3,numOfDet) = noise_estimate;  %%% estimated noise
            end

        else     %%%  two-sided comparision for right section with wrap
            if i >= Xlength-guardLen+1
                noise_estimate = (sum(Xcube(i-guardLen-noiseWin:i-guardLen-1))...
                    + sum(Xcube(guardLen+i-Xlength+1:guardLen+i-Xlength+noiseWin)))/N;
            else
                noise_estimate = (sum(Xcube(i-guardLen-noiseWin:i-guardLen-1))...
                    + sum(Xcube(guardLen+i+1:Xlength))+sum(Xcube(1:noiseWin-Xlength+i+guardLen)))/N;
            end

            if Xcube(i) > alpha*noise_estimate
                numOfDet = numOfDet + 1;
                Detect(1,numOfDet) = i; %%% index
                Detect(2,numOfDet) = Xcube(i);  %%% object power
                Detect(3,numOfDet) = noise_estimate;  %%% estimated noise
            end
        end
    end
end
end