function res = reduce(im,bfilt)
%
% function res = reduce(im,bfilt)
%
% Separable convolution and subsampling by a factor of two
% im: input image
% bfilt: convolution kernel (vector).
%       Default is: [.0625 .25 .375 .25 .0625]'
% res: result image
%
% res is 1/2 the size of im.  Fills NaNs for invalid pixels near
% edges.

if ~exist('bfilt')
  bfilt=[.0625 .25 .375 .25 .0625]';
end

% Use standard Matlab convolution routines instead for ease of
% distribution, and to set edge values to NaNs
bsize = floor(length(bfilt)/2);
tmp1 = conv2sep(im,bfilt,bfilt,'valid');
tmp2 = NaN*ones(size(im));
tmp2(1+bsize:size(im,1)-bsize,1+bsize:size(im,2)-bsize)=tmp1;
res = tmp2(1:2:size(im,1),1:2:size(im,2));

return;

%%%%%%%%%
% Debug %
%%%%%%%%%

foo=pgmRead('einstein.pgm');
bar=reduce(foo);
bar=replaceValue(bar,NaN,0);
displayImage(bar);

in=ones(7,7)
res=reduce(in)


%DEMO_PASSGP Demonstration of PASS-GP method for GP classification
%
%  Description
%    Here we demonstrate PASS-GP method for Gaussian Processes
%    classification. Data used is 2-dimensional toy data with Gaussian
%    bumbs defining classes. We demonstrate with both fixed and not fixed
%    sizes of active set for PASS-GP.
%
%    PASS-GP uses a predictive active set selection method by Henao &
%    Winther (2012) to select a subset of training data to be used for
%    inference in classification problems.
%
%  Reference:
%    Ricardo Henao & Ole Winther (2012). Predictive active set selection
%    methods for Gaussian processes. Neurocomputing 80 (2012), 10-18.
%
%  See also PASSGP
%
% Copyright (c) 2013 Ville Tolvanen

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

% Generate toy data
prevstream=setrandstream(0);
[x1,x2]=meshgrid(-5:0.1:5,-5:0.1:5);
x=[x1(:) x2(:)]; x=x(randperm(size(x,1),3000),:);
y=2.*mnorm_pdf(x, [0 0], [0.5 0;0 0.5]) + mnorm_pdf(x, [3 3], [0.5 0;0 0.5]) + mnorm_pdf(x, [-3 -3], [0.5 0;0 0.5]);
y=y+mnorm_pdf(x, [3 -3], [0.5 0;0 0.5])+mnorm_pdf(x, [-3 3], [0.5 0;0 0.5]);
y=y+0.03.*randn(size(y,1),1);
y(y>0.15)=1; y(y<=0.15)=-1;

[xt1, xt2]=meshgrid(-5:0.23:5,-5:0.23:5);
xt=[xt1(:) xt2(:)];
yt=ones(size(xt,1),1);

[n, nin] = size(x);

% Define covariance and likelihood functions and create the model
gpcf = gpcf_sexp();
lik=lik_probit();
gp=gp_set('lik', lik, 'cf', gpcf, 'jitterSigma2', 1e-6);

opt=optimset('TolX',1e-3,'TolFun',1e-3,'display','on');
w0=gp_pak(gp);

% fPASS-GP with fixed size of 800 points in active set and data divided to
% 10 subsets with 4 sweeps over data
start=tic;[gp, indA]=passgp(gp, x, y, 'opt', opt, 'npass', 4, 'ninit', 800, 'nsub', 10, 'display', 'on', 'fixed', 'on', 'pexc', 0.1, 'optimn', 2);time=toc(start);
tt=time;
[Eft, Varft, lpyt, Eyt, Varyt]=gp_pred(gp, x(indA,:), y(indA,:), xt, 'yt', yt);
figure, [cc,hh]=contour(reshape(xt(:,1),size(xt1,1), size(xt1,1)), reshape(xt(:,2),size(xt1,1), size(xt1,1)), reshape(exp(lpyt),size(xt1,1), size(xt1,1)), [0.1 0.9]);
clabel(cc,hh); title('Pr(y==1) (fpass-gp)')
param=gp_pak(gp);

% PASS-GP with inclusion threshold 0.65, deletion threshold 0.99, intial
% size of 400 points in active set and 3 sweeps over data.
gp=gp_unpak(gp,w0);
start=tic;[gp, indA2]=passgp(gp, x, y, 'opt', opt, 'pinc', 0.65, 'pdel', 0.99, 'npass', 3, 'ninit', 400, 'nsub', 10, 'display', 'on', 'optimn', 2);time=toc(start);
tt2=time;
[Eft2, Varft2, lpyt2, Eyt2, Varyt2]=gp_pred(gp, x(indA2,:), y(indA2,:), xt, 'yt', yt);
figure, [cc,hh]=contour(reshape(xt(:,1),size(xt1,1), size(xt1,1)), reshape(xt(:,2),size(xt1,1), size(xt1,1)), reshape(exp(lpyt2),size(xt1,1), size(xt1,1)), [0.1 0.9]);
clabel(cc,hh); title('Pr(y==1) (pass-gp)')
param2=gp_pak(gp);

% Full Gaussian process for comparison
gp=gp_unpak(gp,w0);
opt.Display='iter';
start=tic;gp=gp_optim(gp,x,y,'opt',opt);tt3=toc;
[Eft3, Varft3, lpyt3, Eyt3, Varyt3]=gp_pred(gp, x, y, xt, 'yt', yt);
figure, [cc,hh]=contour(reshape(xt(:,1),size(xt1,1), size(xt1,1)), reshape(xt(:,2),size(xt1,1), size(xt1,1)), reshape(exp(lpyt3),size(xt1,1), size(xt1,1)), [0.1 0.9]);
clabel(cc,hh); title('Pr(y==1) (full gp)')

% Display some statistics
mlpd_fpassgp=mean(mean(lpyt,2))
time_fpassgp=mean(tt)
mlpd_passgp=mean(mean(lpyt2,2))
time_passgp=mean(tt2)
mlpd_full=mean(lpyt3)
time_full=tt3

% Plot data and active sets for both methods
figure(4), subplot(1,2,1),  plot(x(y==1,1),x(y==1,2),'or',x(y==-1,1),x(y==-1,2),'ob');
hold all; plot(x(indA,1), x(indA,2), '.k'); title('Data and active set (fpass-gp)')
legend('y=1', 'y=-1', 'Active set for fpass-gp');
subplot(1,2,2),  plot(x(y==1,1),x(y==1,2),'or',x(y==-1,1),x(y==-1,2),'ob');
hold all; plot(x(indA2,1), x(indA2,2), '.k'); title('Data and active set (pass-gp)')
legend('y=1', 'y=-1', 'Active set for pass-gp');
setrandstream(prevstream);


function a=update_a(dvec,n,alpha)
% a=update_a(dvec,n, alpha)
% Computes update for a_ki (KxI) in variational approximation (Buntine & Jakulin
% DCA 2006)
% dvec: data - each column is one image (J x I)
% n: (J x K x I)
% alpha, beta: parameters of the Gamma distribution for latent h (1xK each)
% J=#pixels
% K=#components
% I=#images.

[j,k,i]=size(n);

dvec_reshaped = reshape(dvec,j,1,i); % (Jx1xI)
atmp = sum(bsxfun(@times,dvec_reshaped,n),1); % (1xKxI)
a=bsxfun(@plus, squeeze(atmp), alpha'); % (KxI)


% op_CSIRemoveLipids.m
%
% Removes lipids from CSI data using L2 regularization. This minimizes the
% equation norm(x - x_0, 2) + beta * norm(W'x, 2)
%
% INPUT:
% MRSIStruct        = MRSI structure used in FID-A
% lipidComponents   = number of lipid spectra in the lipid basis
% lineWidthRange    = range of linewidth used for building lipid basis
% ppmRange          = ppm range used in building lipid basis
% beta              = regularization term
% plotBasis         = plot basis spectra
%
%
% OUTPUT:
% MRSIStruct        = MRSI structure with lipids removed

function [MRSIStruct] = op_CSIRemoveLipids(MRSIStruct, basisArguments, plottingArguments)
    arguments
        MRSIStruct (1, 1) struct
        basisArguments.lipidComponenets (1, 1) double = 1000
        basisArguments.lineWidthRange (1, 2) double = [1 80]
        basisArguments.lipidPPMRange (1, 2) double = [0.3 1.9000]
        basisArguments.beta (1, 1) double = 1e-4
        plottingArguments.plotBasis (1, 1) logical = false
    end
    % extract arguments from name value pairs
    lipidComponents = basisArguments.lipidComponenets;
    lineWidthRange = basisArguments.lineWidthRange;
    lipidPPMRange = basisArguments.lipidPPMRange;
    beta = basisArguments.beta;

    spectraSize = getSizeFromDimensions(MRSIStruct, {'t'});

    % calculate lipid basis used for L2 regularization
    lipidBasis = createLipipBasis(MRSIStruct, lipidComponents, lineWidthRange, lipidPPMRange);
    if(plottingArguments.plotBasis)
        figure
        plot(MRSIStruct.ppm, flip(real(lipidBasis),1));
    end
    % calculate solution from the basis
    L2Solution = inv(eye(spectraSize) + beta * (lipidBasis * lipidBasis'));

    [MRSIStruct, prevPermute, prevShape] = reshapeDimensions(MRSIStruct, {'t'});
    data = getData(MRSIStruct);

    data = L2Solution * data;
    MRSIStruct = setData(MRSIStruct, data);
    MRSIStruct = reshapeBack(MRSIStruct, prevPermute, prevShape);
end

% calculate lipid basis
function lipidBasis = createLipipBasis(MRSIStruct, lipidComponents, lineWidthRange, lipidPPMRange)
    spectralWidth = getSpectralWidth(MRSIStruct);

    spectralPoints = getSizeFromDimensions(MRSIStruct, {'t'});
    fidBasis = zeros(spectralPoints, lipidComponents);

    lipidStructure = load('Lip.mat', 'sysLip');
    lipidStructure = lipidStructure.sysLip;

    for iSpectra = 1:lipidComponents

        fidBasis(:, iSpectra) = getRandomLipidFids(spectralPoints, spectralWidth, ...
                                                  lineWidthRange, lipidPPMRange, ...
                                                  lipidStructure);
    end
    lipidBasis = fftshift(fft(fidBasis, [], 1), 1);
end

% calculate the single lipid spectra for the basis
function lipidFids = getRandomLipidFids(spectralPoints, spectralWidth, lineWidth, ...
                                        lipidPPMRange, lipidSystem)
    [randomLineWidth, randomPPM] = getRandomLineWidthandPPM(lineWidth, lipidPPMRange);
    lipidSystem.shifts = randomPPM;

    simulatedSignal = sim_onepulse(spectralPoints, spectralWidth, 3, randomLineWidth, lipidSystem);
    simulatedSignal = op_complexConj(simulatedSignal);
    simulatedSignal = addRandomPhase(simulatedSignal);
    %simulatedSignal = scaleSpectra(simulatedSignal, randomPPM, lipidPPMRange);
    lipidFids = simulatedSignal.fids;
end

% pick a random number from lower bounds and upper bounds
function randomNumber = randomNumberInRange(lowerBounds, upperBounds)
    difference = upperBounds - lowerBounds;
    randomNumber = lowerBounds + difference * rand(1);
end

function simulatedSignal = addRandomPhase(simulatedSignal)
    sepctralPhase = randomNumberInRange(-180, 180);
    simulatedSignal = op_addphase(simulatedSignal, sepctralPhase, 0, 4.65, 1);
end

% scale spectra based on a normal distribution. Signal near the center of lipid
% range will be scaled high and signal near the edges scaled lower.
function simulatedSignal = scaleSpectra(simulatedSignal, lipidPPM, lipidRange)
    normalProbability = normpdf(lipidPPM, mean(lipidRange), diff(lipidRange)/4);
    simulatedSignal = op_ampScale(simulatedSignal, normalProbability);
    simulatedSignal = op_ampScale(simulatedSignal, 10);

end

function [randomLineWidth, randomPPM] = getRandomLineWidthandPPM(lineWidth, lipidPPMRange)
    randomLineWidth = randomNumberInRange(lineWidth(1), lineWidth(2));
    randomPPM = randomNumberInRange(lipidPPMRange(1), lipidPPMRange(2));
end


% Prabhakar. S
% AE 110 Lab Assignment
% Satellite Tracking Program
% Satellite:  NOAA 12
% Program Name elaz : Elevation-Azimuth being called by the program sts

% Orbital Elements ready for degree to radians conversion!
% Two Line Elements and the Epoch Data are obtained from the GUI Interface

load variables.mat; % Loading the variables from the GUI Interface sts
I2 = yoe;		% Year of epoch
J2 = moe;			% Month of epoch
%h2 = hoe;       % Hour of epoch
%m2 = mioe;       % Minute of epoch
%K2 = doe + hoe/24 + mioe/(24*60);		% Day of epoch
K2=doe;
zero=0; % For printing a single digit month or day preceeded by a zero.
%ut=.08235372;
%frac=.08235372; % fraction of the day from the TLE

% This calculation is to find the Julian Date,
% JD Fortran code written by Fliegel and Van Flandern [1968]
% Handout by Dr. P

JD = (367*I2 - (fix(7*(I2 + fix((J2 + 9)/12))/4)) + fix((275*J2)/9) + K2 + 1721013.5 + frac);

%JD = (K2 - 32075 + round(1461 * (I2 + 4800 + (J2 -14)/12)/4 ...
%+ 367* (J2 - 2 - (J2 - 14)/12*12)/12 - 3*((I2 + 4900 + (J2 - 14)/12)/100)/4)) ;
% fprintf('\n Two Line Elements Data - 28 Dec 2000, Time 11:30:00\n');
% fprintf(' The Julian Date is %f',JD);

% Propagation begins
I = yoe;								% Year
J = moe;								% Month
%h = hoe;   				   		% Hour
%m = mioe;     				 		% Minute
%K = doe + hoe/24 + mioe/(24*60);	% Day
K=doe;

JD0 = (367*I - (fix(7*(I + fix((J + 9)/12))/4)) + fix((275*J)/9) + K + 1721013.5 + frac);

%JD0 = (K - 32075 + round(1461 * (I + 4800 + (J -14)/12)/4 ...
%+ 367 * (J - 2 - (J - 14)/12*12)/12 - 3*((I + 4900 + (J - 14)/12)/100)/4)) ;

I3 = yoe;							% Year
J3 = moe;							% Month
%h3 = hoe;       					% Hour
%m3 = mioe;       				% Minute
%K3 = (doe+pro) + h3/24 + m3/(24*60);	% Day
K3 = (doe+pro);

JD1 = (367*I3 - (fix(7*(I3 + fix((J3 + 9)/12))/4)) + fix((275*J3)/9) + K3 + 1721013.5 + frac);

%JD1 = K3 - 32075 + round(1461 * (I3 + 4800 + (J3 -14)/12)/4 ...
%  + 367 * (J3 - 2 - (J3 - 14)/12*12)/12 - 3*((I3 + 4900 + (J3 - 14)/12)/100)/4);
% Propagation ends here.

% Propagation step Size
step = 5/(24*60); % right now for every 5 minutes
j=1;

fprintf('\n\t\t\t SATELLITE    TRACKING    SYSTEM  \n');
fprintf(' ---------------------------------------------------------------------------');
fprintf('\n				             G r e g o r i a n   D a t e \n');
fprintf(' ---------------------------------------------------------------------------');
fprintf('\n  Julian Date      Azimuth     Elevation  Month/Day/Year Hour:Minute:Seconds\n');
fprintf(' ---------------------------------------------------------------------------');

while JD0<JD1;

% Declaring variables and initializing values

degtorad = pi/180;		% Radians conversion factor
i = incl*degtorad;		% Inclination
CapOmega = raan*degtorad;		% Right Ascension of the Ascending Node - denoted by symbol Capital Omega
ecc = e;					% Eccentricity
SmallOmega = aop*degtorad;						% Argument of Periapsis - denoted by  small Omega
Mo = ma * degtorad;		% Initial Mean anomaly
n = revs;	   		% Mean Motion (revs/day)
GP = 3.986e5;				% Gravitational parameter - denoted by nu
a = 24000;					% Semi-major Axis
% a = (GP/n^2)^(1/3);
% fprintf('Semi-major axis is computed to be :\n',a);

Latitude = 0 * degtorad;
Longitude = 280 * degtorad;
L = ((90*degtorad) - Latitude);

We = 6.300388097;		% Angular velocity of planet Earth
perturb = 98.9246096453622 * degtorad; % Perturbation due to the Oblateness
tq = 2448621.5; % JD for
Qg = perturb + We * (JD0 - tq);
Q = Qg + Longitude;

tp = JD - Mo/n;
M = n * (JD0 - tp);

E = M;				          		% Use M for the first value of E
	f=M-E+ecc*sin(E);			 		% Kepler's Equation
	while abs(f)>0.00000001; 		% Loop for Kepler's equation begins
		f=M-E+ecc*sin(E);			 	%
		fd=-1+ecc*cos(E);			 	%
		E=E-f/fd;						%
	end;							 		% loop for Kepler's eqn ends here!

% Computing true anomaly and radius

TA=2*atan((((1+ecc)/(1-ecc))^0.5)*tan(E/2));
    if TA<0;
    	  TA=2*pi+TA;
    end;

r=a*(1-ecc*cos(E)); % radius

% Coordinate transformations

adot = (-(3/2) * n * 1.0827e-3 * (6378.14/a)^2 * cos(i))/(1-ecc^2)^2;
A = CapOmega + adot * (JD0 - JD);
SmallOmega = SmallOmega + adot * (JD0 - JD);

Rseu = [0; 0; 6378.14];
Txyz = [ cos(A) -sin(A) 0; sin(A) cos(A) 0; 0 0 1 ] * [ 1 0 0; 0 cos(i) -sin(i); 0 sin(i) cos(i)] * ...
   [ cos(SmallOmega) -sin(SmallOmega) 0; sin(SmallOmega) cos(SmallOmega) 0; 0 0 1 ];
Tseu = [ cos(L) 0 -sin(L); 0 1 0; sin(L) 0 cos(L)] * [ cos(Q) sin(Q) 0; -sin(Q) cos(Q) 0; 0 0 1];
ruvw = [ (r * cos(TA)); (r * sin(TA)); 0 ];
Pseu = Tseu*(Txyz*ruvw) - Rseu;
P = (Pseu(1)^2 + Pseu(2)^2 + Pseu(3)^2)^0.5;

El = (asin ( Pseu(3)/P ))/degtorad;
Az = (atan2 ( Pseu(2), -Pseu(1)))/degtorad;
	if Az < 0;
   	Az = 360 + Az;
   end;

%Loop begins here  %if El>0;

array(j,1) = JD0;
array(j,3) = Az;
array(j,4) = El;
%array(j,5) = cat(year,month,day);
j = j+1;
% loop ends here

JD0 = JD0 + step;
			% Gregorian Date from Julian Date
			% This calculation is valid for any Julian Day Number including negative JDN and
			% produces a Gregorian date (or possibly a proleptic Gregorian date)

			Z = floor(JD0 - 1721118.5); % JD0 is the Julian Date in propagation
			R = (JD0 - 1721118.5 - Z);  % R is the fractional part of JD0
			G = (Z - .25);
			A = (floor(G / 36524.25));  % Calculate the value of A which is the number of full centuries
			B = (A - (A / 4)); % The value of B is this number of days minus a constant
			year = (floor((B+G) / 365.25)); % Calculate the value of Y, the year in a calendar whose years start on March 1
			C = (B + Z - floor(365.25 * year));  % Day count
         month = (fix((5 * C + 456) / 153));  % Month
         UT = (C - fix((153 * month - 457) / 5) + R); % Calculation for UTC
         day = floor(UT);  % Gregorian Day

						if month > 12;
		      			year = year + 1 ;
		      			month = month - 12;
                  end;
                  UT = UT - floor(UT);
         UT = UT*24;
         hr = floor(UT);   %  hour
         UT = UT-floor(UT);
	      UT =UT* 60;
	      min = floor(UT);  % minute
   	   UT =UT- floor(UT);
      	UT =UT* 60;
         secs = round(UT); % seconds
         if secs==60;
            min=min+1;
            secs=0;
         end;
         % Gregorian Date conversion algorithm ends here

         % computations for printintg two digits in case year,month,day,min and secs is < 10
         % (i.e) to print third month, 3 as 03 and day 5 as 05 etc..
         ZERO=int2str(zero);
			MIN=int2str(min); % converting minute into a string
			if min <10;
		  			 MinNew=strcat(ZERO,MIN);
                  min=MinNew;
               else;
                  min=MIN;
               end;

         	DAY=int2str(day); % converting DAY into a string
				 if day <10;
		  				 DayNew=strcat(ZERO,DAY);
                   day=DayNew;
                else;
                  day=DAY;
         	 end;
          HR=int2str(hr); % converting Hour into a string
					if hr <10;
		  				 HrNew=strcat(ZERO,HR);
                   hr=HrNew;
               else;
                 	 hr=HR;
		          end;

                MONTH=int2str(month); % converting month into a string
						if month <10;
			  				 MonthNew=strcat(ZERO,MONTH);
   	                month=MonthNew;
            	   else
         	         month=MONTH;
                  end;

                 SECS=int2str(secs); % converting seconds into a string
						if secs <10;
			  				 SecsNew=strcat(ZERO,SECS);
   	                secs=SecsNew;
            	   else
         	         secs=SECS;
			         end;

         % computations for printing two digits ends here!

         if El>0;
            fprintf('\n %f | %f | %f |  %s/%s/%4d  |  %s:%s:%s  |',JD0, Az, El, month,day,year, hr,min,secs);
            %plot(El,Az);
         end;
end;
fprintf('\n ---------------------------------------------------------------------------\n');
fprintf('\n\t\t\t       End of Output\!!! \n\n\n');


function [ a, info ] = dpofa ( a, lda, n )

%*****************************************************************************80
%
%% DPOFA factors a real symmetric positive definite matrix.
%
%  Discussion:
%
%    DPOFA is usually called by DPOCO, but it can be called
%    directly with a saving in time if RCOND is not needed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(LDA,N), the symmetric matrix to be  factored.  Only the
%    diagonal and upper triangle are used.
%
%    Input, integer LDA, the leading dimension of the array A.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real A(LDA,N), an upper triangular matrix R so that A = R'*R
%    where R' is the transpose.  The strict lower triangle is unaltered.
%    If INFO /= 0, the factorization is not complete.
%
%    Output, integer INFO, error flag.
%    0, for normal return.
%    K, signals an error condition.  The leading minor of order K is not
%    positive definite.
%
  for j = 1 : n

    s = 0.0;

    for k = 1 : j-1
      t = a(k,j) - ddot ( k-1, a(1:k-1,k), 1, a(1:k-1,j), 1 );
      t = t / a(k,k);
      a(k,j) = t;
      s = s + t * t;
    end

    s = a(j,j) - s;

    if ( s <= 0.0 )
      info = j;
      return
    end

    a(j,j) = sqrt ( s );

  end

  info = 0;

  return
end


function c = long2intval(C)
%LONG2INTVAL  Conversion long to intval (with correct rounding)
%
%  c = long2intval(C)
%

% written  12/30/98     S.M. Rump
% modfied  02/09/00     S.M. Rump  rounding unchanged after use
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 07/31/05     S.M. Rump  header corrected
% modified 11/20/05     S.M. Rump  fast check for rounding to nearest
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  INTLAB_LONG_BETA = getappdata(0,'INTLAB_LONG_BETA');
  INTLAB_LONG_LOGBETA = getappdata(0,'INTLAB_LONG_LOGBETA');
  INTLAB_LONG_ERROR = getappdata(0,'INTLAB_LONG_ERROR');

  % extra treatment of zero component
  indexzero = ( C.exponent==-inf );

  n = size(C.mantissa,1);
  % take maximum first 80 bits
  precC = size(C.mantissa,2);
  p = min( precC , ceil(80/INTLAB_LONG_LOGBETA)+1 );
  if INTLAB_LONG_ERROR
    if p<precC
      C.error = errorupdate( 1 , C.error , 0 , ...
                  1 , any(C.mantissa(:,p+1:precC)~=0,2) , C.exponent-precC );
    end
    Cerr = C.error.mant .* INTLAB_LONG_BETA.^(C.error.exp-C.exponent+p);
  else
    Cerr = 0;
  end

  factor = ( INTLAB_LONG_BETA.^(-p:-1) )';
  exppos = ( C.exponent>=0 );
  expneg = ~exppos;
  E = floor( 600/INTLAB_LONG_LOGBETA );
  F = INTLAB_LONG_BETA ^ E;

  Cmantp = C.mantissa(:,p);
  setround(-1)
  C.mantissa(:,p) = Cmantp - Cerr;
  cinf = C.mantissa(:,p:-1:1) * factor ;
  if any(exppos)
    cinf(exppos) = ( cinf(exppos)*F ) .* ...
                      ( INTLAB_LONG_BETA.^(C.exponent(exppos)-E) );
  end
  if any(expneg)
    cinf(expneg) = ( cinf(expneg)/F ) .* ...
                      ( INTLAB_LONG_BETA.^(C.exponent(expneg)+E) );
  end

  setround(1)
  C.mantissa(:,p) = Cmantp + Cerr;
  csup = C.mantissa(:,p:-1:1) * factor ;
  if any(exppos)
    csup(exppos) = ( csup(exppos)*F ) .* ...
                      ( INTLAB_LONG_BETA.^(C.exponent(exppos)-E) );
  end
  if any(expneg)
    csup(expneg) = ( csup(expneg)/F ) .* ...
                      ( INTLAB_LONG_BETA.^(C.exponent(expneg)+E) );
  end
  if INTLAB_LONG_ERROR & any(indexzero)
    csup(indexzero) = C.error.mant(indexzero) .* ...
                        INTLAB_LONG_BETA.^C.error.exp(indexzero);
    cinf(indexzero) = - csup(indexzero);
  end

  c = hull( C.sign.*cinf , C.sign.*csup );

  setround(rndold)


function [Rsp,U,V]=standardUVBeamPattern(T,xyPoints,normVals,Sigma,lineParams,bounds,numPoints)
%%STANDARDUVBEAMPATTERN Compute the beam pattern of a tapered array as a
%                       function of direction given in terms of direction
%                       cosines for a narrowband linear or planar array.
%                       The beam pattern is just the sum of all of the
%                       tapered elements taken for signals in different
%                       directions. This function can output the response
%                       over all (-1,+1) u-v values or in a rectangular
%                       subset. It can also output a linear cut across the
%                       values.
%
%INPUTS: T The numSubarraysXnumElements tapering matrix of the array. If
%          there are no subarrays, then this can be a numElementsX1
%          vector of weights for every element. The weights can be complex,
%          which means that difference beams can be formed and steering can
%          be taken into account. If an empty matrix is passed, then it is
%          assumed that no tapering is used so T will be a 1XnumElements
%          vector of all ones.
% xyPoints A 1XNumDim or 2XnumDim matrix of the [x;y] locations of the
%          points in the linear or planar array. The units of the distances
%          are in terms of wavelengths for the narrowband model.
% normVals This indicates how the sum beam value should be normalized.
%          Possible values are:
%          'ArrayGain' Return the ratio of the output power to the noise
%                      power. This is the default if this parameter is
%                      omitted or an empty matrix is passed.
%          'NormPowGain' Return the power of the output normalized such
%                        that the highest value is 1.
%          'AbsPowGain' Return the power of the output. This assumes that
%                       the tapering matrix contains the true gain or loss
%                       values for the tapering and is thus not scaled by
%                       any constant value.
%          'NormRealVal' Display the normalized real component of the
%                       output value. It is normalized such that the
%                       largest absolute value is one. Note that this is
%                       not squared, so it corresponds to an output
%                       voltage, not a power.
%           'RawOutput' Provide the raw sum beam output.
%   Sigma If the array gain is desired (normVals='ArrayGain'), then this
%         parameter is used. This is the numElsXnumEls covariance matrix of
%         the noise at the individual elements in the array. If this
%         parameter is omitted or an empty matrix is passed, then the
%         identity matrix will be used.
% lineParams If xyPoints is a 2XnumDims set of points (for a planar array),
%         then if this parameter is provided and is not an empty matrix,
%         Rsp will be a 1D cut across u and v values rather than all u and
%         v values. The equation for the line along with the beam pattern
%         will be evaluated is v=lineParams.intercept+lineParams.slope*u if
%         the value lineParams.vIndep=false or the vIndep component of
%         lineParams is omitted. Otherwise, the line is
%         u=lineParams.intercept+lineParams.slope*v .
%  bounds A 2X1 (or 1X2) or a 4X1 (or 1X4) vector with the bounds in u and
%         v of the plot. For 1-dimensional plots, which are the case if
%         xyPoints is 1XnumDim or lineParams is provided and xyPoints is
%         2XnumDim, then bounds=[minVal;maxVal] for the independent
%         variable. For two-dimensional plots, then
%         bounds=[minU;maxU;minV;maxV]. If this parameter is omitted or an
%         empty matrix is passed, then [-1;1;-1;1] is used to go over all
%         possible values in u and v (or just u for a 1D plot).
% numPoints A parameter determining the size of the output matrix. For 1D
%         plots, Rsp is numPointsX1. For 2D plots, Rsp is
%         numPointsXnumPoints. If this parameter is omitted or an empty
%         matrix is passed, then a default of numPoints=125 points is used.
%
%OUTPUTS: Rsp The array beam pattern over the selected region, normalized
%             as specified. The value in entry Rsp(i,j) corresponds to the
%             U and V values U(i,j) and V(i,j). For a 1D response for a
%             linear array, Rsp(i) corresponds to U(i) and if xyPoints is
%             2XnumDims, V(i) is the corresponding v value. The value of
%             Rsp for points outside of the visible region (u^2+v^2>1)
%             is set to zero.
%           U A matrix of points corresponding to the u values of the
%             responses in Rsp.
%           V A matrix of points corresponding to the v values of the
%             responses in Rsp. For 1D responses,
%
%The idea of a beam pattern for a narrowband array is discussed in Chapter
%2.2 of [1]. The array gain is discussed in Chapter 2.6.2.
%
%EXAMPLE 1:
%Here, we find the array gain beam pattern of a 20 element 1D linear array
%with Taylor tapering and half-wavelength element spacing. When consdiering
%the array gain, it does not matter that the elements are not provided
%centered about the origin. However, the points must be centered to get the
%proper tapering values from the TaylorLinearTapering function.
% xPoints=0:0.5:9.5;
% xPoints=xPoints-mean(xPoints);
% nBar=3;
% sidelobedB=-25;
% T=TaylorLinearTapering(nBar,sidelobedB,xPoints).';
% [Rsp,U]=standardUVBeamPattern(T,xPoints);
% figure(1)
% clf
% plot(U,10*log10(Rsp),'linewidth',2)
% axis([-1 1 -30 15])
% axis square
% h1=xlabel('u');
% h2=ylabel('Array Gain');
% title('Array Power Gain in Decibels')
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%EXAMPLE 2:
%In this example, we plot the normalized power gain of a 2D hexagonal array
%without any tapering.
% xyPoints=getShaped2DLattice([7;14],'hexagonal');
% [Rsp,U,V]=standardUVBeamPattern([],xyPoints,'NormPowGain');
% figure(2)
% clf
% surface(U,V,10*log10(Rsp),'EdgeColor','None')
% axis([-1 1 -1 1 -40 0])
% caxis([-40 0])
% colormap(jet(256))
% colorbar()
% view(45,45)
% light()
% h1=xlabel('u');
% h2=ylabel('v');
% h3=zlabel('Array Gain');
% title('Array Power Gain in Decibels')
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h3,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%EXAMPLES 3:
%In this example, we plot the difference pattern of a circular array.
%First, we plot the normalized real value of the pattern (the pattern is
%real anyway so this is not an issue) in 2D and then we take 1D cuts of it
%in two different directions. Bayliss tapering are used to obtain the
%difference pattern.
% xyPoints=getShaped2DLattice([30;30],'circular');
% sidelobedB=-30;
% N=17;
% T=BaylissTapering(sidelobedB,N,xyPoints).';
% [Rsp,U,V]=standardUVBeamPattern(T,xyPoints,'NormRealVal');
% figure(3)
% clf
% surface(U,V,Rsp,'EdgeColor','None')
% axis([-1 1 -1 1 -1 1])
% caxis([-1 1])
% colormap(jet(256))
% colorbar()
% view(45,10)
% light()
% h1=xlabel('u');
% h2=ylabel('v');
% h3=zlabel('Real Response');
% title('Array Difference  Beam Pattern')
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h3,'FontSize',14,'FontWeight','bold','FontName','Times')
%
% %Now, we take a cut along to v=0 line.
% lineParams=[];
% lineParams.intercept=0;
% lineParams.slope=0;
% lineParams.vIndep=false;
%
% [Rsp,U]=standardUVBeamPattern(T,xyPoints,'NormRealVal',[],lineParams);
% figure(4)
% clf
% plot(U,Rsp,'linewidth',2)
% h1=xlabel('u');
% h2=ylabel('Real Response');
% title('Array Difference Beam Pattern')
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
% %And here we take a cut along the u=0 line.
% lineParams.vIndep=true;
%
% [Rsp,~,V]=standardUVBeamPattern(T,xyPoints,'NormRealVal',[],lineParams);
% figure(5)
% clf
% plot(V,Rsp,'linewidth',2)
% h1=xlabel('v');
% h2=ylabel('Real Response');
% title('Array Difference Beam Pattern')
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%EXAMPLE 4:
%This is an example of using elements of a circular array with a fixed
%Taylor tapering that have been broken into subarrays. We then try to form
%the best approximation to a Bayliss difference pattern modifying only the
%subarray outputs. We also demonstrate how steering can be used to move the
%difference beam away from the center of the array.
% xyPoints=getShaped2DLattice([30;30],'circular');
% numEls=size(xyPoints,2);
% %Fixed element-level tapering.
% sidelobedB=-30;
% nBar=4;
% g=TaylorTapering(nBar,sidelobedB,xyPoints);
% %It is assumed the disjoint subarrays can be formed (no adjacency matrix
% %used).
% numLevels=3;
% N=17;
% T=double(findBaylissSubarrays(xyPoints,numLevels,sidelobedB,N));
% %We now apply the tapering to the matrix
% T=bsxfun(@times,T,g.');
%
% %The desired element-level tapering
% g=BaylissTapering(sidelobedB,N,xyPoints);
%
% %Next, we try to find the best subarray weights to approximate the desired
% %tapering. We specifically add a null at the boresight.
% g=findSubarrayWeights(T,g,ones(numEls,1));
%
% %Apply the tapering to the elements
% T=bsxfun(@times,g,T);
%
% %We also steer the array off boresight to u0. This is element-level
% %steering.
% u0=[-0.5;0.5];
% D=diag(exp(1j*2*pi*sum(bsxfun(@times,xyPoints,u0),1)));
% T=T*D;
%
% [Rsp,U,V]=standardUVBeamPattern(T,xyPoints,'NormRealVal');
% figure(3)
% clf
% surface(U,V,Rsp,'EdgeColor','None')
% axis([-1 1 -1 1 -1 1])
% caxis([-1 1])
% colormap(jet(256))
% colorbar()
% view(45,10)
% light()
% h1=xlabel('u');
% h2=ylabel('v');
% h3=zlabel('Real Response');
% title('Array Difference  Beam Pattern')
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h3,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] H. L. Van Trees, Optimum Array Processing. New York: Wiley-
%    Interscience, 2002.
%
%August 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numDim=size(xyPoints,1);
numEls=size(xyPoints,2);

if(isempty(T))
   T=ones(1,numEls);
end

if(nargin<7||isempty(numPoints))
   numPoints=125;
end

if(nargin<6||isempty(bounds))
    if(numDim==1)
        bounds=[-1;1];
    else
        bounds=[-1;1;-1;1];
    end
end

if(nargin<5||isempty(lineParams))
    if(numDim>1)
        %If a 2D plot is desired.
        uVals=linspace(bounds(1),bounds(2),numPoints);
        vVals=linspace(bounds(3),bounds(4),numPoints);
        [U,V]=meshgrid(uVals,vVals);
        Rsp=zeros(numPoints,numPoints);

        if(isvector(T))
            for curVal=1:(numPoints*numPoints)
                u=[U(curVal);V(curVal)];
                if(u'*u>1)
                    continue;
                end

                Rsp(curVal)=T*exp(-1j*2*pi*sum(bsxfun(@times,xyPoints,u),1)).';
            end
        else
            for curVal=1:(numPoints*numPoints)
                u=[U(curVal);V(curVal)];
                if(u'*u>1)
                    continue;
                end

                Rsp(curVal)=sum(sum(T*exp(-1j*2*pi*sum(bsxfun(@times,xyPoints,u),1)).'));
            end
        end
    else
        %If a 1D plot is required.
        U=linspace(bounds(1),bounds(2),numPoints)';
        V=[];

        if(isvector(T))
            Rsp=zeros(numPoints,1);
            for curVal=1:numPoints
                if(abs(U(curVal))>1)
                    continue;
                end

                Rsp(curVal)=T*exp(-1j*2*pi*xyPoints*U(curVal)).';
            end
        else
            Rsp=zeros(numPoints,1);
            for curVal=1:numPoints
                if(abs(U(curVal))>1)
                    continue;
                end

                Rsp(curVal)=sum(T*exp(-1j*2*pi*xyPoints*U(curVal)).');
            end
        end
    end
else
    %If a 1D cut across the 2D surface is desired. The
    slope=lineParams.slope;
    intercept=lineParams.intercept;
    if(isfield(lineParams,'vIndep'))
        vIndep=lineParams.vIndep;
    else
        vIndep=false;
    end

    if(vIndep==false)
        %The independent variable is u.
        U=linspace(bounds(1),bounds(2),numPoints)';
        V=zeros(numPoints,1);
        Rsp=zeros(numPoints,1);

        if(isvector(T))
            for curVal=1:numPoints
                V(curVal)=intercept+slope*U(curVal);
                uVec=[U(curVal);V(curVal)];

                if(uVec'*uVec>1)
                    continue;
                end

                Rsp(curVal)=T*exp(-1j*2*pi*sum(bsxfun(@times,xyPoints,uVec),1)).';
            end
        else
            for curVal=1:numPoints
                V(curVal)=intercept+slope*U(curVal);
                uVec=[U(curVal);V(curVal)];

                if(uVec'*uVec>1)
                    continue;
                end

                Rsp(curVal)=sum(sum(T*exp(-1j*2*pi*sum(bsxfun(@times,xyPoints,uVec),1)).'));
            end
        end
    else
        %The independent variable is v.
        V=linspace(bounds(1),bounds(2),numPoints)';
        U=zeros(numPoints,1);
        Rsp=zeros(numPoints,1);

        if(isvector(T))
             for curVal=1:numPoints
                U(curVal)=intercept+slope*V(curVal);
                uVec=[U(curVal);V(curVal)];

                if(uVec'*uVec>1)
                    continue;
                end

                Rsp(curVal)=T*exp(-1j*2*pi*sum(bsxfun(@times,xyPoints,uVec),1)).';
            end
        else
            for curVal=1:numPoints
                U(curVal)=intercept+slope*V(curVal);
                uVec=[U(curVal);V(curVal)];

                if(uVec'*uVec>1)
                    continue;
                end

                Rsp(curVal)=sum(sum(T*exp(-1j*2*pi*sum(bsxfun(@times,xyPoints,uVec),1)).'));
            end
        end
    end
end

if(nargin<3||isempty(normVals))
    normVals='ArrayGain';
end

switch(normVals)
    case 'ArrayGain'%Array power gain versus spatially white noise.
        if(nargin<4||isempty(Sigma))
           Sigma=eye(numEls,numEls);
        end

        if(isvector(T))
            Rsp=abs(Rsp).^2/sum(T*Sigma*T');
        else

            numSubarrays=size(T,1);
            e=ones(numSubarrays,1);
            %The sum sums up all of the subarray outputs.
            Rsp=abs(Rsp).^2/sum(e'*T*Sigma*T'*e);
        end
    case 'NormPowGain'%Normalized absolute array power gain
        Rsp=abs(Rsp);
        Rsp=(Rsp/max(Rsp(:))).^2;%Make it relative to the peak.
    case 'AbsPowGain'
        Rsp=abs(Rsp)^2;
    case 'NormRealVal'%Display the real component of the response.
        Rsp=real(Rsp);
        Rsp=Rsp/max(Rsp(:));%Make it relative to the peak.
    case 'RawOutput'
    otherwise
        error('Unknown plot type requested')
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function Y = variantMultiply( psfMatData, X, padsize );
%
%           Y = variantMultiply( psfMatData, X, padsize );
%
%  This function computes the multiplication of a spatially variant
%  point spread function (PSF) times an image:
%                y = A*x
%
%  Here we assume A is made up of a several space invariant PSFs, and
%  use piece-wise constant interpolation of them to define the spatially
%  variant PSF.  That is, A has the form:
%
%       A = D1*A1 + D2*A2 + ... + Dp*Ap
%
%  Input:
%   psfMatData  -  cell array containing the matrix data of each of the
%                  individual PSFs.  This matrix data is usuall computed
%                  from onePsfMatrix.m
%            X  -  array containing the image to which the psfMatrix
%                  is to be multiplied.
%
%  Output:
%            Y  -  contains the result after PSF multiplication.
%

%  J. Nagy 1/7/02

imsize = size( X ) - 2*padsize;

%
%  We partition the image domain into regions of equal sizes,
%  according to the number of PSFs we have ...
%
nregions = size(psfMatData);
rsize = ceil(imsize ./ nregions);

%
%  In order for this to be consistent for 2-D and 3-D images, we need to make
%  sure there is a third dimension ...
%
if length(imsize) == 1
  imsize = [imsize, 1, 1];
  rsize = [rsize, 1, 1];
  nregions = [nregions, 1, 1];
  padsize = [padsize, 0, 0];
elseif length(imsize) == 2
  imsize = [imsize, 1];
  rsize = [rsize, 1];
  nregions = [nregions, 1];
  padsize = [padsize, 0];
end

%
%  Coding the rest of this will be easier if all of the image subregions
%  have the same dimensions.  If it's not, we pad with a few zeros to make
%  it so ...
%
padsize1 = rsize .* nregions - imsize;
if any( padsize1 < 0 )
  error('Something is wrong here ...')
end
X = padarray(X, padsize1, 'post');

%
%  Now we get information about beginning and ending indices of subregions
%  so we can "put" and "get" subregions correctly ...
%
[RIidx, RJidx, RKidx] = region_indices( nregions, rsize );
[EIidx, EJidx, EKidx] = eregion_indices( RIidx, RJidx, RKidx, 2*padsize );

%
%  Now loop over all the subregions ...
%
Y = zeros(imsize);
for k = 1:nregions(3)
  for j = 1:nregions(2)
    for i = 1:nregions(1)
      Xt = X(EIidx(i,1):EIidx(i,2), EJidx(j,1):EJidx(j,2), EKidx(k,1):EKidx(k,2));
      Yt = invariantMultiply( psfMatData{i,j,k}, Xt, padsize(1:length(size(X))) );
      Y(RIidx(i,1):RIidx(i,2), RJidx(j,1):RJidx(j,2), RKidx(k,1):RKidx(k,2)) = Yt;
    end
  end
end

Y = Y(1:imsize(1), 1:imsize(2), 1:imsize(3));


classdef BT8 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP with bias feature

%------------------------------- Reference --------------------------------
% H. Li, Q. Zhang, and J. Deng, Biased multiobjective optimization and
% decomposition algorithm, IEEE Transactions on Cybernetics, 2017, 47(1):
% 52-66.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            [N,D] = size(X);
            I1    = 2 : 2 : D;
            I2    = 3 : 2 : D;
            Y     = X - repmat(X(:,1),1,D).^(0.5+1.5*repmat(0:D-1,N,1)/(D-1));
            DY    = Y.^2 + (1-exp(-Y.^2/1e-3))/5;
            PopObj(:,1) = X(:,1)         + sum(4*DY(:,I1).^2-cos(8*pi*DY(:,I1))+1,2);
            PopObj(:,2) = 1-sqrt(X(:,1)) + sum(4*DY(:,I2).^2-cos(8*pi*DY(:,I2))+1,2);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - R(:,1).^0.5;
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


function [ know, x ] = p18_sol ( n )

%*****************************************************************************80
%
%% P18_SOL returns the solution for problem 18.
%
%  Discussion:
%
%    The solution values are taken from Brent.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2002
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the problem.  This value
%    is only needed for those problems with variable N.
%
%    Output, integer KNOW.
%    If KNOW is 0, then the solution is not known.
%    If KNOW is positive, then the solution is known, and is returned in X.
%
%    Output, real X(N), the solution, if known.
%
  if ( n == 2 )
    know = 1;
    x = [ 0.2113249, 0.7886751 ]';
  elseif ( n == 4 )
    know = 1;
    x = [ 0.1026728, 0.4062037, 0.5937963, 0.8973272 ]';
  elseif ( n == 6 )
    know = 1;
    x = [ 0.066877, 0.288741, 0.366682, 0.633318, ...
      0.711259, 0.933123 ]';
  elseif ( n == 8 )
    know = 1;
    x = [ 0.043153, 0.193091, 0.266329, 0.500000, ...
      0.500000, 0.733671, 0.806910, 0.956847 ]';
  else
    know = 0;
    x = zeros ( n, 1 );
  end

  return
end


function [lm lm_avg] = rcu_ach(n, delta, epsil, plow, pup);
% Compute RCU achievability bound.
% lm returns the bound valid for maximal probability of error
% lm_avg is for the average
%
% This is not a vectorized version, because the function is really slow.
% You can speed it up significantly if you provide a bracket plow, pup for the value of logm.
% For example, a smart idea is to set
% plow = gallager_ach(n, delta, epsil) and pup = converse(n,delta,epsil);

if nargin < 5
	plow = 0;
	pup = n;
end

eps_test = precise_rand(n, delta, pup);
if(eps_test < epsil)
	disp(sprintf([	'-- achiev_prand(n = %d, delta = %g, epsil = %g): eps_test = %g\n'...
			'        This is a bug? precise_rand() contradicts converse ?!?!'], ...
			n, delta, epsil, eps_test));
	error('achiev_prand');
	lm_avg = 0; lm = 0;
	return;
end

eps_test = precise_rand(n, delta, plow);
if(eps_test > epsil)
	disp(sprintf([	'-- achiev_prand(n = %d, delta = %g, epsil = %g): eps_test = %g\n'...
			'        Can not find lower bound for precise_rand() !!!!'], ...
			n, delta, epsil, eps_test));
	%error('achiev_prand');
	lm_avg = 0; lm = 0;
	return;
end

% Take into account that we are computing AVERAGE prob. of error,
% and so we need a random linear code trick to go to maximum => log M must be integer
while floor(plow) < floor(pup);
	ptest = (plow + pup)/2;
	eps_test = precise_rand(n, delta, ptest);
	if(eps_test > epsil)
		pup = ptest;
	else
		plow = ptest;
		% This is not needed as we stop on floor-test
		%if  (epsil-eps_test)< 1e-2*epsil
		%	break;
		%end
	end
end
lm_avg = plow;
lm = floor(plow);


function L = probitNoiseLikelihood(noise, mu, varsigma, y)

% PROBITNOISELIKELIHOOD Likelihood of the data under the PROBIT noise model.
% FORMAT
% DESC returns the likelihood of a data set under the  probit based classification noise model.
% ARG noise : the noise structure for which the likelihood is required.
% ARG mu : input mean locations for the likelihood.
% ARG varSigma : input variance locations for the likelihood.
% ARG y : target locations for the likelihood.
%
% SEEALSO : probitNoiseParamInit, probitNoiseLogLikelihood, noiseLikelihood
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005

% NOISE

D = size(y, 2);
for i = 1:D
  mu(:, i) = mu(:, i) + noise.bias(i);
end
L = cumGaussian((y.*mu)./(sqrt(noise.sigma2+varsigma)));


function [ a, seed ] = r8vec_uniform ( n, b, c, seed )

%*****************************************************************************80
%
%% R8VEC_UNIFORM returns a scaled pseudorandom R8VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, real B, C, the range of the pseudorandom values.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real A(N), the vector of pseudorandom values.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_UNIFORM - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R8VEC_UNIFORM - Fatal error!' );
  end

  r = zeros ( n, 1 );

  for i = 1 : n

    k = floor ( seed / 127773 );

    seed = 16807 * ( seed - k * 127773 ) - k * 2836;

    if ( seed < 0 )
      seed = seed + 2147483647;
    end

    a(i) = b + ( c - b ) * seed * 4.656612875E-10;

  end

  return
end


function bf = cubic_spline_fourier(f, a)

% The continuous Fourier transform of a cubic spline kernel.

bf = -(- 12*a + 12*exp(-pi*f*2i) + 12*exp(pi*f*2i) + 6*a*exp(-pi*f*4i) + ...
    6*a*exp(pi*f*4i) + f.*(pi*exp(-pi*f*2i)*12i) - f.*(pi*exp(pi*f*2i)*12i) + ...
    a*f.*(pi*exp(-pi*f*2i)*16i) - a*f.*(pi*exp(pi*f*2i)*16i) + ...
    a*f.*(pi*exp(-pi*f*4i)*4i) - a*f.*(pi*exp(pi*f*4i)*4i) - 24)./(16*f.^4*pi^4);

bf(f == 0) = 1;


% MANIPULATOR TRAJECTORY GENERATION
% Generates combined transform (rotation and translation) trajectories
% using custom time scaling from a separate trajectory.
%
% Copyright 2019 The MathWorks, Inc.

%% Setup
clear, clc, close all

% Define waypoint information
createWaypointData;

% Define IK
ik = inverseKinematics('RigidBodyTree',gen3);
ikWeights = [1 1 1 1 1 1];
ikInitGuess = gen3.homeConfiguration;

% Set up plot
plotMode = 2; % 0 = None, 1 = Trajectory, 2 = Coordinate Frames
show(gen3,gen3.homeConfiguration,'Frames','off','PreservePlot',false);
xlim([-1 1]), ylim([-1 1]), zlim([0 1.2])
hold on
if plotMode == 1
    hTraj = plot3(waypoints(1,1),waypoints(2,1),waypoints(3,1),'b.-');
end
plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ro','LineWidth',2);

%% Generate and follow trajectory
% Loop through segments one at a time
trajType = 'trap';
numWaypoints = size(waypoints,2);
for w = 1:numWaypoints-1

    % Get the initial and final transforms and times for the segment
    T0 = trvec2tform(waypoints(:,w)') * eul2tform(orientations(:,w)');
    Tf = trvec2tform(waypoints(:,w+1)') * eul2tform(orientations(:,w+1)');
    timeInterval = waypointTimes(w:w+1);
    trajTimes = timeInterval(1):ts:timeInterval(2);

    % Generate time scaling trajectory for the segment on the range [0 1]
    switch trajType
        case 'trap'
            [s,sd,sdd] = trapveltraj([0 1],numel(trajTimes), ...
                                     'EndTime',diff(timeInterval));
        case 'cubic'
            [s,sd,sdd] = cubicpolytraj([0 1],timeInterval,trajTimes);
        case 'quintic'
            [s,sd,sdd] = quinticpolytraj([0 1],timeInterval,trajTimes);
        otherwise
            error('Invalid trajectory type! Use ''trap'', ''cubic'', or ''quintic''');
    end

    % Find the transforms from trajectory generation
    [T,vel,acc] = transformtraj(T0,Tf,timeInterval,trajTimes, ...
                                'TimeScaling',[s;sd;sdd]);

    % Trajectory visualization for the segment
    if plotMode == 1
        eePos = tform2trvec(T);
        set(hTraj,'xdata',eePos(:,1),'ydata',eePos(:,2),'zdata',eePos(:,3));
    elseif plotMode == 2
        plotTransforms(tform2trvec(T),tform2quat(T),'FrameSize',0.05)
    end

    % Trajectory following for the segment
    for idx = 1:numel(trajTimes)
        % Solve IK
        tgtPose = T(:,:,idx);
        [config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess);
        ikInitGuess = config;

        % Show the robot
        show(gen3,config,'Frames','off','PreservePlot',false);
        title(['Trajectory at t = ' num2str(trajTimes(idx))])
        drawnow
    end

end


function [gal] = km32gal(km3)
% Convert volume from cubic kilometers to US liquid gallons.
% Chad Greene 2012
gal = km3*264172052360;


function A = sim_genVARModelFromEq(expr,morder)
%
% Generate VAR model coefficient matrix from text-based system of equations
% The model can then be realized using the tvarsim() function modified from
% ARfit [2].
% If the expr specification requests time-varying coefficients, then this
% function will return inline function placeholders for coefficients, which
% can be evaluated to actual coefficient matrices by sim_genTVARcoeffs().
%
% Inputs:
%
%   expr:       A cell vector containing each equation as a string (one
%               equation per cell element). See Examples for format.
%   morder:     The model order
%
% Outputs:
%
%   A:          VAR[p] model coefficients in format A=[A1,A2, ... Ap] where
%               p = morder. Ai is the M x M coefficient matrix for lag i.
%
% Example1: generate a static VAR[3] model from a (text-based) system of equations
%
% expr = { ...
%     'x1(t) = 0.9*x1(t-1)  + 0.3*x2(t-2)  + e1(t)' ...
%     'x2(t) = 1.3*x2(t-1)  + -0.8*x2(t-2) + e2(t)' ...
%     'x3(t) = 0.3*x1(t-2)  + 0.6*x2(t-1)  + e3(t)' ...
%     'x4(t) = -0.7*x4(t-3) + -0.7*x1(t-3) + 0.3*x5(t-3) + e4(t)' ...
%     'x5(t) = 1*x5(t-1)    + -0.4*x5(t-2) + 0.3*x4(t-2) + e5(t)' ...
%     };
% A = sim_genVARModelFromEq(expr,3)
% A =
%     0.9000         0         0         0         0         0    0.3000         0         0         0         0         0         0         0         0
%          0    1.3000         0         0         0         0   -0.8000         0         0         0         0         0         0         0         0
%          0    0.6000         0         0         0    0.3000         0         0         0         0         0         0         0         0         0
%          0         0         0         0         0         0         0         0         0         0   -0.7000         0         0   -0.7000    0.3000
%          0         0         0         0    1.0000         0         0         0    0.3000   -0.4000         0         0         0         0         0
%
% % Now generate some data from the model (this requires arsim.m or tvarsim.m)
%
% M   = size(A,1);        % number of variables
% Nl  = 1000;             % length of each trial
% Nr  = 100;              % number of trials
% C = eye(M);             % specify the covariance matrix
% data = zeros(M,Nl,Nr);
% for tr=1:Nr             % simulate data from VAR model
%    data(:,:,tr) = arsim(zeros(1,M),A,C,Nl)';
% end
% eegplot(data,'srate',1); % visualize the simulated data
%
%
% Example 2: generate a time-varying VAR[2] model
%
%
% See Also: tvarsim(), est_fitMVAR(), sim_genTVARcoeffs()
%
% References:
%
% [1] Mullen T (2010) The Source Information Flow Toolbox (SIFT):
%   Theoretical Handbook and User Manual. Section 6.5.1
%   Available at: http://www.sccn.ucsd.edu/wiki/Sift
% [2] Schneider T, Neumaier A (2001) Algorithm 808: ARfit---a matlab package
%   for the estimation of parameters and eigenmodes of multivariate
%   autoregressive models. ACM Transactions on Mathematical Software 27:58-65
%   http://www.gps.caltech.edu/~tapio/arfit/
%
% Author: Tim Mullen 2010, SCCN/INC, UCSD.
% Email:  tim@sccn.ucsd.edu

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

Nvars = length(expr);

if nargin>1
    % create cell array of zeros
    A = repmat({0},Nvars,Nvars*morder);
end

fnstr ='{[\S]+}';

for i = 1:Nvars
    eq = expr{i};

    % Identify all inline functions
    % Specifically, we extract any string f(x) encapsulated by
    % curly-brackets '{f(x)}'
    [matchstr] = regexp(eq,'{[\S]+}','match');

    % replace each function with an identifier '$i' designating that the
    % ith equation will go here
    for fi=1:length(matchstr)
        eq = strtrim(strrep(eq,matchstr{fi},sprintf('$%d',fi)));
    end

    % remove all whitespace
    eq(isspace(eq))=[];
%
%     for fi=1:length(matchstr)
%         eq = [eq(1:matchstart(fi)-1) sprintf('$%d',fi) eq(matchend(fi)+1:end)];
%     end

    terms = regexp(eq,'([=+*])','split');

    row = str2double(regexp(terms{1},'(\d+)','match'));
    for k = 2:2:length(terms)-1
        vars = regexp(terms{k+1},'(\d+)','match');
        vars = str2num(char(vars));
        col = vars(1)+(vars(2)-1)*Nvars;

        if strcmpi(terms{k}(1),'$')
            % term is a function expression
            A{row,col} = matchstr{str2double(terms{k}(2))}(2:end-1);
        elseif ~isnan(str2double(terms{k}))
            % term is a number
            A{row,col} = str2double(terms{k});
        else
            % term is an expression to be directly evaluated
            A{row,col} = eval(terms{k});
        end
    end
end

% if A is all numeric, convert to standard array
if all(cellfun(@(x)isnumeric(x),A))
    A = cell2mat(A);
end


function [ edge_data, node_num2 ] = tet_mesh_order4_to_order10_size ( ...
  tetra_num, tetra_node1, node_num1 )

%*****************************************************************************80
%
%% TET_MESH_ORDER4_TO_ORDER10_SIZE sizes a quadratic tet mesh from a linear one.
%
%  Discussion:
%
%    A quadratic (10 node) tet mesh can be derived from a linear
%    (4 node) tet mesh by interpolating nodes at the midpoint of
%    every edge of the mesh.
%
%    The mesh is described indirectly, as the sum of individual
%    tetrahedrons.  A single physical edge may be a logical edge of
%    any number of tetrahedrons.  It is important, however, that a
%    new node be created exactly once for each edge, assigned an index,
%    and associated with every tetrahedron that shares this edge.
%
%    This routine handles that problem.
%
%    The primary amount of work occurs in sorting a list of 6 * TETRA_NUM
%    data items, one item for every edge of every tetrahedron.  Each
%    data item records, for a given tetrahedron edge, the global indices
%    of the two endpoints, the local indices of the two endpoints,
%    and the index of the tetrahedron.
%
%    Through careful sorting, it is possible to arrange this data in
%    a way that allows the proper generation of the interpolated nodes.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 November 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer TETRA_NUM, the number of tetrahedrons in the
%    linear mesh.
%
%    Input, integer TETRA_NODE1(4,TETRA_NUM), the nodes that make up
%    each tetrahedron in the linear mesh.
%
%    Input, integer NODE_NUM1, the number of nodes for the linear mesh.
%
%    Output, integer EDGE_DATA(5,6*TETRA_NUM), edge data.
%
%    Output, integer NODE_NUM2, the number of nodes for the quadratic mesh.
%

%
%  Step 1.
%  From the list of nodes for tetrahedron T, of the form: (I,J,K,L)
%  construct the six edge relations:
%
%    (I,J,1,2,T)
%    (I,K,1,3,T)
%    (I,L,1,4,T)
%    (J,K,2,3,T)
%    (J,L,2,4,T)
%    (K,L,3,4,T)
%
%  In order to make matching easier, we reorder each pair of nodes
%  into ascending order.
%
  for tetra = 1 : tetra_num

    i = tetra_node1(1,tetra);
    j = tetra_node1(2,tetra);
    k = tetra_node1(3,tetra);
    l = tetra_node1(4,tetra);

    [ a, b ] = i4i4_sort_a ( i, j );

    edge_data(1:5,6*(tetra-1)+1) = [ a, b, 1, 2, tetra ]';

    [ a, b ] = i4i4_sort_a ( i, k );

    edge_data(1:5,6*(tetra-1)+2) = [ a, b, 1, 3, tetra ]';

    [ a, b ] = i4i4_sort_a ( i, l );

    edge_data(1:5,6*(tetra-1)+3) = [ a, b, 1, 4, tetra ]';

    [ a, b ] = i4i4_sort_a ( j, k );

    edge_data(1:5,6*(tetra-1)+4) = [ a, b, 2, 3, tetra ]';

    [ a, b ] = i4i4_sort_a ( j, l );

    edge_data(1:5,6*(tetra-1)+5) = [ a, b, 2, 4, tetra ]';

    [ a, b ] = i4i4_sort_a ( k, l );

    edge_data(1:5,6*(tetra-1)+6) = [ a, b, 3, 4, tetra ]';

  end
%
%  Step 2. Perform an ascending dictionary sort on the neighbor relations.
%  We only intend to sort on rows 1:2; the routine we call here
%  sorts on the full column but that won't hurt us.
%
%  What we need is to find all cases where tetrahedrons share an edge.
%  By sorting the columns of the EDGE_DATA array, we will put shared edges
%  next to each other.
%
  edge_data = i4col_sort_a ( 5, 6*tetra_num, edge_data );
%
%  Step 3. All the tetrahedrons which share an edge show up as consecutive
%  columns with identical first two entries.  Figure out how many new
%  nodes there are, and allocate space for their coordinates.
%
  node_num2 = node_num1;

  n1_old = -1;
  n2_old = -1;

  for edge = 1 : 6 * tetra_num
    n1 = edge_data(1,edge);
    n2 = edge_data(2,edge);
    if ( n1 ~= n1_old | n2 ~= n2_old )
      node_num2 = node_num2 + 1;
      n1_old = n1;
      n2_old = n2;
    end
  end

  return
end


function Main()
% This program is a stand-alone toy example of radiation treatment planning
% (RTP) optimization for a brain tumor case.
%     The program generates a toy patient head model using scaled and shifted
% ellipsoids and p-norm sublevel sets to represent all the volumes of
% interest (VOIs), including skin, eyes, optic nerves, brain stem, a tumor
% and artificial "shell" around the tumor. These VOI sublevel sets are then
% discretized into PointClouds, by retaining the points in a discrete 3-D
% grid of volume elements (voxels) that lie in the 1-sublevel set of the
% VOI's p-norm model.
%     Then we compute candidate beam directions by firing beams at random
% points on the tumor surface from a set of about 150 nodes, located
% uniformly in a spherical region of radius 80cm from the patient's head.
%     Then we compute the dose vectors associated with each beam in each of
% the VOIs, by evaluating a dose function in a cylindrical region around each
% beam direction. The dose function is a crude model of the roll-off with
% depth, and radial diffusion or scatter. For each candidate beam, this
% rolloff/scatter function is evaluated for each voxel in each VOI. Thus
% each beam results in a column of the patient dose matrix.
% Finally, we set the min and max dosages for each VOI.
%     We then pass the dose matrices and min and max specs to one of two
% solvers (CPLEX or ADMM), which find the optimal set of beams and
% intensities (beam weights) for this patient case. This is the optimal
% "plan". The optimization formulation is based on a problem from the
% Stanford EE364b final exam from 2011, by Stephen Boyd and Eric Chu.
%     Then we visualize the optimal plan by plotting the dose levels for
% all the voxels of each VOI, and the dose volume histograms.
%     The program consists of this Main() function and several "classes":
% (1) Patient: cell array of VOIs, names, etc
% (2) VOI: p-norm model and associated point cloud and boundary, dose specs
% (3) Beams: beam heads,tails,dose vector computation + nodes + collimators
% (4) Model: p-norm models and gradients
% (5) Point Cloud & 3-D geometry functions
% (6) Optimizer functions: CPLEX wrapper and ADMM solver (ala EE364b)
%
% Reference:
% H. Hindi, "A Tutorial on Optimization Methods for Cancer Radiation
% Treatment Planning," Proc. American Contr. Conf, 2013.
%
% Written by Haitham Hindi, 2012/01/18
%
% DISCLAIMER: This code is intended for algorithm research purposes
% only!! Author makes no claims as to the realism, accuracy, or correctness
% of ANY part of this code, including (but not limited to):
% patient anatomy and dimensions; plan safety; beam weights,doses,physics;
% dose units, dose specs; plan quality, evaluation metrics. The author is
% an engineer with no medical training whatsoever! This code is supplied
% as-is, with no guarantee of correctness, and the author accepts no
% responsibility for any damage or false conclusions from the use of this
% code. Of course, we would appreciate hearing about any bugs you might find.

    clear, clc, close all
    rng('default');
    rng(2);

    %====================================================
    % data for patient and beams
    %====================================================
    % patient specs
    PatientName     = 'Toy Brain Tumor Case';
    PatientThetaxyz = [pi/2;0;0];
    PatientOffset   = [0;0;0];
    PatientAxLims   = [-15 15];

    % VOI specs (VOIs are scaled shifted p-norm sublevel sets)
    Target=1;Shell=2;BrStm=3;EyeL=4;EyeR=5;OptNrvL=6;OptNrvR=7;Skin=8;
    VoxSizeSkin      = 1;
    VoxSizeOrgans    = 0.2;%0.15%0.113;
    SampFactorSkin   = 1;
    SampFactorOrgans = 1;
    VOINames      ={'Target','Shell','BrStm','EyeL','EyeR','OptNrvL','OptNrvR','Skin'};
    dmaxs         =[50      , 25    ,20     , 20   , 20   , 20      , 20      , 30];
    dmins         =[40      , 0     , 0     ,  0   ,  0   ,  0      ,  0      ,  0];
    VOINumbers    =[ 1,       2,      3,      4,     5,     6,        7,        8];
    VOITypes      ={'v',     's',    'v',    'v',   'v',   'v',      'v',      's'};
    VOIColors     ={'k',     'r',    'g',    'b',   'b',   'c',      'c',      'y'};
    VOILineStyles ={'-',     '-',    '-',    '-',   '--',  '-',      '--',     '-'};
    VOIVoxSizes   =[VoxSizeOrgans*ones(7,1);VoxSizeSkin];
    VOISampFactors=[SampFactorOrgans*ones(7,1);SampFactorSkin];
    VOIxyzScales  =[1.2*[1;1;1],1.56*[1;1;1],[1.5;1.5;6],1.7*[1;1;1],1.7*[1;1;1],[0.7;4;0.7],[0.7;4;0.7],[11;11;11]];
    VOIpNorms     =[2;  2;  3;  2;  2;  2;  2;  3];
    VOIThetaxyz   =[[0;0;0],[0;0;0],[-pi/8;0;0],[0;0;0],[0;0;0],[pi/6;-pi/6;0],[pi/6;pi/6;0],[0;0;0]];
    VOIOffsets    =[[3;-3;0],[3;-3;0],[0;-3;-5],[-4;8;4],[4;8;4],[-2;3;2],[2;3;2],[0;0;0]];
    VOIHistBins   = 0:max(dmaxs)/50:1.4*max(dmaxs);

    % beam specs
    NPtsSphereParam = 7%8%18
    NodeLocs = GetPointsOnUnitSphere1000(NPtsSphereParam);
    NodeLocs = NodeLocs(find(NodeLocs(:,3)>=-0.6),:);% remove bottom (can't go under patient)
    NodeLocs = NodeLocs(find(NodeLocs(:,2)<= 0.6),:);% remove front  (can't go through patient)
    NodeLocs = 80*NodeLocs;% scale to 80cm radius
    NNodes = size(NodeLocs,1)
    NodesAxLims = 85*[-1 1 -1 1 -1 1];
    Collimators = [5; 7.5; 10; 12.5; 15; 20; 25];%; 30; 35; 40; 50; 60];
    Collimators = Collimators/10; % scale to cm
    NCollimators= length(Collimators);
    NBeamlets = 5;
    NPtsPerAx = 50;
    AxLength = 15;

    %====================================================
    % create patient and beams, solve optimization problem, visualize results
    %====================================================
    % get patient
    disp(['Computing patient model...'])
    Patient = PatientCreate();
    Patient = PatientInit(Patient,PatientName,PatientAxLims,...
                          VOINames,VOINumbers,VOITypes,VOIColors,VOILineStyles,...
                          VOIVoxSizes,VOISampFactors,VOIHistBins,...
                          VOIxyzScales,VOIpNorms,VOIThetaxyz,VOIOffsets);
    Patient = PatientRotateTranslate(Patient,PatientThetaxyz,PatientOffset);

    figure
    PatientPlot(Patient)

    % get beams
    disp(['Computing beam directions...'])
    Beams = BeamsCreate();
    Beams = BeamsInit(Beams,NodeLocs,NodesAxLims,Collimators,NBeamlets,Patient.VOIs{Skin}.Model.f,Patient.VOIs{Target}.PointCloudBdy);

    PlotDoseMapAxialRolloffAndRadialDiffusion(Collimators);
    PlotPatientBeamDirectionsAndNodes(Patient,Beams);
    %figure,hold on
    %PatientPlot(Patient)
    %[GridPts,AxPts] = GetGridPts(NPtsPerAx,AxLength);
    %PlotFewRandomBeamDoseMaps(Beams.Tails,Beams.Heads,Beams.Widths,3,GridPts)

    % compute dose matrices of all VOIs and set upper and lower bounds
    Patient = PatientPrepareForOptimization(Patient,Beams,dmins,dmaxs);

    % construct the Linear Program matrices and solve for optimal beam
    % weights using CLEX or ADMM
    Patient = PatientApplyOptimization(Patient);

    % plot optimal beam weights, constraints, and dose volume histograms
    PatientEvaluate(Patient);

return

%====================================================
% Patient Class: mainly cell array of VOIs, names, axis limits
%====================================================
function Patient = PatientCreate()
    Patient.Name     = [];
    Patient.VOINames = [];
    Patient.NumVOIs  = [];
    Patient.VOIs     = [];
    Patient.AxLims   = [];
    Patient.xBeamWeights = [];
return

function Patient = PatientInit(Patient,PatientName,PatientAxLims,...
                               VOINames,VOINumbers,VOITypes,VOIColors,VOILineStyles,...
                               VOIVoxSizes,VOISampFactors,VOIHistBins,...
                               VOIxyzScales,VOIpNorms,VOIThetaxyz,VOIOffsets)
    Patient.Name = PatientName;
    Patient.VOINames = VOINames;
    Patient.NumVOIs =length(VOINames);
    Patient.VOIs = {};
    for i=1:Patient.NumVOIs
        Modi            = ModelInit(ModelCreate(),VOIxyzScales(:,i),VOIpNorms(i));
        VOIi            = VOIInit(VOICreate(),VOINames{i},VOINumbers(i),VOITypes{i},VOIColors{i},VOILineStyles{i},VOIVoxSizes(i),VOISampFactors(i),Modi,VOIHistBins);
        Patient.VOIs{i} = VOIRotateTranslate(VOIi,VOIThetaxyz(:,i),VOIOffsets(:,i));
    end
    Patient.AxLims = PatientAxLims;
return

function Patient = PatientPrepareForOptimization(Patient,Beams,dmins,dmaxs)
    for i=1:Patient.NumVOIs
        Patient.VOIs{i} = VOIGetDoseAMatrix(Patient.VOIs{i},Beams);
        Patient.VOIs{i} = VOISetDoseMinMax(Patient.VOIs{i},dmins(i),dmaxs(i));
    end
return

function Patient = PatientApplyOptimization(Patient)
    % construct LP matrices
    A    = [];dmin = [];dmax = [];
    for i=1:Patient.NumVOIs
        A    = [A;Patient.VOIs{i}.ADose];
        dmin = [dmin;Patient.VOIs{i}.DoseMin];
        dmax = [dmax;Patient.VOIs{i}.DoseMax];
    end

    % compute optimal beam weights
    %%%Patient.xBeamWeights = OptimizeBeamsCPLEX(A,dmin,dmax);
    Patient.xBeamWeights = OptimizeBeamsADMM(A,dmin,dmax);

    % compute resulting dose on each VOI
    for i=1:Patient.NumVOIs
        Patient.VOIs{i} = VOIGetDose(Patient.VOIs{i},Patient.xBeamWeights);
    end
return

function PatientEvaluate(Patient)
    figure,stem(Patient.xBeamWeights),title('Beam Weights')
    figure,PatientPlotConstraints(Patient);
    figure,PatientPlotDVH(Patient);
return

function Patient = PatientRotateTranslate(Patient,Thetaxyz,Offset)
    for i=1:Patient.NumVOIs
        Patient.VOIs{i} = VOIRotateTranslate(Patient.VOIs{i},Thetaxyz,Offset);
    end
return

function Patient = PatientAffineTransform(Patient,A,b)
    for i=1:Patient.NumVOIs
        Patient.VOIs{i} = VOIAffineTransform(Patient.VOIs{i},A,b);
    end
return

function PatientPlot(Patient)
    hold on
    for i=1:Patient.NumVOIs
        VOIPlot(Patient.VOIs{i},Patient.AxLims);
    end
    title(Patient.Name),legend(Patient.VOINames)
    xlabel('x'),ylabel('y'),zlabel('z')
    view(127.5,30)
    %view(3)
return

function PatientPlotConstraints(Patient)
    nPlotsPerCol = ceil(Patient.NumVOIs/2);
    for i=1:Patient.NumVOIs
        subplot(nPlotsPerCol,2,i),VOIPlotMinMaxConstraints(Patient.VOIs{i});
    end
return

function PatientPlotDVH(Patient)
    hold on,title('DVHs'),grid on, axis([0,max(Patient.VOIs{1}.DVHBins),0,1.1])
    for i=1:Patient.NumVOIs
        [DVHi,Histi] = GetDVH(Patient.VOIs{i}.Dose,Patient.VOIs{i}.DVHBins);
        plot(Patient.VOIs{i}.DVHBins,DVHi,Patient.VOIs{i}.Color);
    end
    legend(Patient.VOINames)
return

function PlotPatientBeamDirectionsAndNodes(Patient,Beams)
    figure,hold on
    PatientPlot(Patient)
    PlotNodes(Beams.NodeLocs,Beams.NodesAxLims);
    PlotBeams(Beams.Tails0,Beams.Heads,1);
    figure,hold on
    PatientPlot(Patient)
    PlotBeams(Beams.Tails,Beams.Heads,1);
return

%====================================================
% VOI Class: has name etc, point cloud model, point cloud, dose matrix, dose specs
%====================================================
function VOI = VOICreate()
    VOI.Name          = [];
    VOI.Number        = [];
    VOI.Type          = [];
    VOI.Color         = [];
    VOI.LineStyle     = [];
    VOI.VoxSize       = [];
    VOI.SubSampFactor = [];
    VOI.Model         = [];
    VOI.PointCloud    = [];
    VOI.PointCloudBdy = [];
    VOI.ADose         = [];
    VOI.DoseMin       = [];
    VOI.DoseMax       = [];
    VOI.Dose          = [];
    VOI.DVHBins       = [];
return

function VOI = VOIInit(VOI,Name,Number,Type,Color,LineStyle,VoxSize,SubSampFactor,Model,HistBins)
    VOI.Name          = Name;
    VOI.Number        = Number;
    VOI.Type          = Type;
    VOI.Color         = Color;
    VOI.LineStyle     = LineStyle;
    VOI.VoxSize       = VoxSize;
    VOI.SubSampFactor = SubSampFactor;
    VOI.Model         = Model;
    VOI.PointCloud    = ModelToPointCloud(Model,VoxSize,SubSampFactor);
    VOI.PointCloudBdy = ModelToPointCloudBdy(Model,VoxSize,SubSampFactor);
    VOI.ADose         = [];
    VOI.DoseMin       = [];
    VOI.DoseMax       = [];
    VOI.Dose          = [];
    VOI.DVHBins       = HistBins;
return

function VOI = VOIAffineTransform(VOI,A,b)
    VOI.Model         = ModelAffineTransform(VOI.Model,A,b);
    VOI.PointCloud    = PointCloudAffineTransform(VOI.PointCloud,A,b);
    VOI.PointCloudBdy = PointCloudAffineTransform(VOI.PointCloudBdy,A,b);
return

function VOI = VOIRotateTranslate(VOI,Thetaxyz,Offset)
    VOI.Model         = ModelRotateTranslate(VOI.Model,Thetaxyz,Offset);
    VOI.PointCloud    = PointCloudRotateTranslate(VOI.PointCloud,Thetaxyz,Offset);
    VOI.PointCloudBdy = PointCloudRotateTranslate(VOI.PointCloudBdy,Thetaxyz,Offset);
return

function VOIPlot(VOI,AxisLimits)
    Symbol = ['.',VOI.Color];
    PlotCloud(VOI.PointCloudBdy,Symbol,AxisLimits);
return

function VOI = VOIGetDoseAMatrix(VOI,Beams)
    if VOI.Type =='v' % full volume
        VOI.ADose = GetBeamDosageVectors8(Beams.Tails,Beams.Heads,Beams.Widths,VOI.PointCloud);
    else              % boundary shell
        VOI.ADose = GetBeamDosageVectors8(Beams.Tails,Beams.Heads,Beams.Widths,VOI.PointCloudBdy);
    end
    disp(['NPts-',VOI.Name,'=',num2str(size(VOI.ADose,1))])
return

function VOI = VOISetDoseMinMax(VOI,dmin,dmax)
    if VOI.Type =='v' % full volume
        NPtsVOI = size(VOI.PointCloud,1);
    else              % boundary shell
        NPtsVOI = size(VOI.PointCloudBdy,1);
    end
    VOI.DoseMin = dmin*ones(NPtsVOI,1);
    VOI.DoseMax = dmax*ones(NPtsVOI,1);
return

function VOI = VOIGetDose(VOI,xBeamWeights)
    VOI.Dose = VOI.ADose*xBeamWeights;
return

function VOIPlotMinMaxConstraints(VOI)
    nVox = length(VOI.DoseMin);
    vox  = 1:nVox;
    plot(vox,VOI.Dose,'*b',...
         vox,VOI.DoseMin,'--r',...
         vox,VOI.DoseMax,'--r')
    axis([0 nVox  0  1.2*max(VOI.DoseMax) ])
    legend(VOI.Name)
    xlabel('voxels'),ylabel('dose')
return

function [DVH,Hist] = GetDVH(DosageProfile,Bins);
    Hist = hist(DosageProfile,Bins);
    Hist = Hist/sum(Hist);
    HistCum = cumsum(Hist);
    DVH = 1-HistCum;
    DVH = [1;DVH(1:(length(DVH)-1))'];
return

%====================================================
% Beam Class: has beams, nodes, collimators, dose computation
%====================================================
function Beams = BeamsCreate()
    Beams.NodeLocs    = [];
    Beams.NodesAxLims = [];
    Beams.Collimators = [];
    Beams.NBeamlets   = [];
    Beams.Tails0= [];
    Beams.Tails = [];
    Beams.Heads = [];
    Beams.Widths= [];
    Beams.Nodes = [];
return

function Beams = BeamsInit(Beams,NodeLocs,NodesAxLims,Collimators,NBeamlets,SkinModelf,BdyTarget)
    Beams.NodeLocs    = NodeLocs;
    Beams.NodesAxLims = NodesAxLims;
    Beams.Collimators = Collimators;
    Beams.NBeamlets   = NBeamlets;
    [BeamTails,BeamHeads,BeamTails0,BeamWidths,BeamNodes] = GetBeamDirections3(SkinModelf,BdyTarget,NodeLocs,Collimators,NBeamlets);
    Beams.Tails0= BeamTails0;
    Beams.Tails = BeamTails;
    Beams.Heads = BeamHeads;
    Beams.Widths= BeamWidths;
    Beams.Nodes = BeamNodes;
return

function [BeamTails,BeamHeads,BeamTails0,BeamWidths,BeamNodes] = GetBeamDirections3(SkinModelf,BdyTarget,NodeLocs,Collimators,NBeamlets)
% compute candidate beam directions by firing from each Node, NBeamlets for each collimator size, onto random points on the target boundary
% also calculate the point of entry into the patient (skin) boundary
    NNodes = size(NodeLocs,1);
    NColls = length(Collimators);
    NBdyTar = size(BdyTarget,1);
    NBisections = 10;
    BeamTails0= zeros(NColls*NNodes*NBeamlets,3);
    BeamTails = zeros(NColls*NNodes*NBeamlets,3);
    BeamHeads = zeros(NColls*NNodes*NBeamlets,3);
    BeamWidths= zeros(NColls*NNodes*NBeamlets,1);
    BeamNodes = zeros(NColls*NNodes*NBeamlets,1);
    % for each node
    for i=1:NNodes
        % make that node the tail for all collimators and beamlets
        ii     = (i-1)*NColls*NBeamlets;
        iiPlus = ii +  NColls*NBeamlets;
        BeamTails0(ii+1:iiPlus,:) = repmat(NodeLocs(i,:),NColls*NBeamlets,1);
        BeamNodes(ii+1:iiPlus,:)  = repmat(i,NColls*NBeamlets,1);
        % for each collimator size
        for j=1:NColls
            % get NBeamlets random heads on target boundary
            jj     = (j-1)*NBeamlets;
            jjPlus = jj  + NBeamlets;
            BeamHeads(ii+jj+1:ii+jjPlus,:) = BdyTarget(GetRandomIndecesNoRepetition(NBeamlets,NBdyTar),:);
            BeamWidths(ii+jj+1:ii+jjPlus,:)= repmat(Collimators(j),NBeamlets,1);
            % for each beamlet
            for k=1:NBeamlets
                % compute intersection with patient surface
                BeamTails(ii+jj+k,:) = IntersectionSegmentLevelset(SkinModelf,BeamTails0(ii+jj+k,:),BeamHeads(ii+jj+k,:),NBisections);
            end
        end
    end
return

function DosageVecs = GetBeamDosageVectors8(Tails,Heads,Widths,PointCloud)%,Collimators)
% compute beam dose matrix on the given PointCloud, and quantizes it to accuracy 1e-3
    NBeams     = size(Tails,1);
    NPts       = size(PointCloud,1);
    DosageVecs = zeros(NPts,NBeams);
    disp(['Computing fake beam dosage vectors...'])
    tic
    for j=1:NBeams
        BeamRadius = Widths(j)/2;
        [DistOnRay,DistToRay]   = ComputeDistancesOnAxisAndToAxis(PointCloud,Tails(j,:)',Heads(j,:)');
        [DistOnRaySelect,DistToRaySelect,SelectIdx] = SelectPositiveOnAxisFiniteOffAxis(DistOnRay,DistToRay);
        DosageVecs(SelectIdx,j) = ComputeFakeBeamRolloffAndDiffusion(DistOnRaySelect,DistToRaySelect,BeamRadius);
    end
    DosageVecs = 1e-3 * round(1e3*DosageVecs); % quantize to 1e-3 accuracy, anything smaller gets set to zero
    toc
return

function DoseVector = ComputeFakeBeamRolloffAndDiffusion(DistOnRay,DistToRay,BeamRadius,BeamDiffusionSigma)
% for single beam, compute fake dose vector as:
% - on-axis build-up then roll-off
% - radial diffusion that broadens a bit with distance along beam direction
    if nargin < 4, BeamDiffusionSigma = 0.2;end
    % [build up then roll off] .* [convolve(rect_aperture,gaussian) = difference of erf's]
    DoseVector =  (DistOnRay+0.02).^(1/8) .* exp(-(DistOnRay+0.02)/15).*...
                  (  erf( (DistToRay+BeamRadius)./(BeamDiffusionSigma*(1+0.025*DistOnRay)) ) - ...
                     erf( (DistToRay-BeamRadius)./(BeamDiffusionSigma*(1+0.025*DistOnRay)) )   );
return

function [DistOnRay,DistToRay] = ComputeDistancesOnAxisAndToAxis(PointCloud,tail,head)
% vectorized computation of cylindrical coordinates along beam direction, for all points in VOI PointCloud,
% used for calculation of VOI dose vector
    NPts        = size(PointCloud,1);
    x0              = tail;
    u0              = ComputeUnitVector(head,tail);
    PointCloudShift = PointCloud-repmat(x0',NPts,1);
    DistOnRay       = PointCloudShift*u0;
    DistToRay       = sqrt( sum(  (PointCloudShift - DistOnRay*u0').^2  ,2)  );
return

function [DistOnRaySelect,DistToRaySelect,SelectIdx] = SelectPositiveOnAxisFiniteOffAxis(DistOnRay,DistToRay)
% keep only points in the positive direction of the beam from the tail0 and
% with distance no more than 5cm from the axis (since largest collimator is 6cm diameter = 3cm radius)
    SelectIdx       = find( (DistOnRay > 0) & (DistToRay <=5) );
    DistOnRaySelect = DistOnRay(SelectIdx);
    DistToRaySelect = DistToRay(SelectIdx);
return

function u = ComputeUnitVector(head,tail)
    u = (head-tail);
    u = u/norm(u);
return

function IndexRand = GetRandomIndecesNoRepetition(NumSamples,NumTotal)
% gets NumSamples random indexes from index set {1,...,NumTotal}, with no repetition
    IndexPerm = randperm(NumTotal);
    IndexRand = IndexPerm(1:NumSamples);
return

function PointOfInt = IntersectionSegmentLevelset(LevelSetf,tail,head,NBisections)
% finds approximate intersection of segment with levelset of f using bisection
% assumes head is inside level set and tail is outside; crashes if not.
    if LevelSetf(tail(:)) <=1, error('tail already inside sublevel set!'),end
    if LevelSetf(head(:)) > 1, error('head is not inside sublevel set!'),end
    for r=1:NBisections
        PointOfInt = 0.5*(tail+head);
        % if PointOfInt inside the 1-level set
        if LevelSetf(PointOfInt(:)) <=1
            head = PointOfInt; % move away from target
        else
            tail = PointOfInt; % move toward target
        end
    end
return

function PlotDoseMapAxialRolloffAndRadialDiffusion(Collimators)
% plot first the roll off of beam intensity along the center of a beam
% then plot a matrix of plots showing the roll off from the axis for all collimators
    BeamRadius  = 0.75/2;
    DistOnRay=0:0.1:50;
    DistToRay=0;
    DoseVector = ComputeFakeBeamRolloffAndDiffusion(DistOnRay,DistToRay,BeamRadius);
    figure
    plot(DistOnRay,DoseVector),grid
    title('On axis beam roll off')
    xlabel('on axis distance [cm]'),ylabel('normalized intensity')

    DistOnRay=0:5:40;
    DistToRay=0:0.1:8;
    NColl = length(Collimators);
    figure,
    for j=1:NColl
        Collj = Collimators(j);
        subplot(4,3,j)
        hold on
        for i=1:length(DistOnRay)
            DoseVector = ComputeFakeBeamRolloffAndDiffusion(DistOnRay(i),DistToRay,Collj/2);
            plot(DistToRay,DoseVector), grid on
        end
        title(['Radial roll off ',num2str(Collj/2),'cm radius beam'])
        xlabel('off axis distance [cm]'),ylabel('normalized intensity')
    end
return

function PlotBeams(BeamTails,BeamHeads,LineWidth)
% plots dotted lines as the directions of the beams (not the full dose vectors)
    if nargin<3, LineWidth=4; end
    for i=1:size(BeamTails,1)
        h = plot3([BeamTails(i,1);BeamHeads(i,1)],...
              [BeamTails(i,2);BeamHeads(i,2)],...
              [BeamTails(i,3);BeamHeads(i,3)]);
        %set(h,'LineWidth',4);
        set(h,'LineWidth',LineWidth,'LineStyle',':');
    end
return

function PlotFewRandomBeamDoseMaps(Tails,Heads,Widths,nBeamsToPlot,GridPts)
% plot few random beams over the points GridPts - rarely do more than 3-5
% beams because can be really slow for a 100x100x100 grid
    NBeams      = length(Widths);
    Select      = GetRandomIndecesNoRepetition(nBeamsToPlot,NBeams);
    TailsSelect = Tails(Select,:);
    HeadsSelect = Heads(Select,:);
    WidthsSelect= Widths(Select,:)
    DosageVecs  = GetBeamDosageVectors8(TailsSelect,HeadsSelect,WidthsSelect,GridPts);
    DosageVecsThresh = 0.01;
    PlotDosageVecs2(DosageVecs,GridPts,DosageVecsThresh);
return

function PlotDosageVecs2(DosageVecs,GridPts,DosageVecsThresh)
% plot the DosageVecs on the grid GridPoints, plotting only values greater
% than DosageVecsThresh
    if nargin < 3, DosageVecsThresh = 0;end
    NDosageVecs = size(DosageVecs,2);
    disp(['Plotting beam dosage vectors...'])
    for j=1:NDosageVecs
        iThresh = find(DosageVecs(:,j) > DosageVecsThresh);
        scatter3(GridPts(iThresh,1),GridPts(iThresh,2),GridPts(iThresh,3),3,DosageVecs(iThresh,j),'filled');
    end
return

function PlotNodes(NodeLocs,NodesAxLims)
    hold on
    plot3(NodeLocs(:,1),NodeLocs(:,2),NodeLocs(:,3),'r.'), grid on
    xlabel('x'),ylabel('y'),zlabel('z')
    axis(NodesAxLims)
    %view(3)
return

%====================================================
% Model Class: for generating point clouds as sublevel sets of scaled/shifted p-norms (1 < p < Inf)
%====================================================
function Model = ModelCreate()
    Model.f = [];
    Model.fGrad    = [];
    Model.AxLims = [];
return

function Model = ModelInit(Model,xyzScale,p)
    A            = diag(1./xyzScale);
    Model.f      = @(x) norm(A*x,p); % scaled p-norm (where 1 < p < Inf)
    Model.fGrad  = @(x) A*(sum((A*x).^p))^((1/p)-1)*(A*x).^(p-1); % grad of scaled p-norm
    Model.AxLims = [xyzScale(1)*[-1 1] xyzScale(2)*[-1 1] xyzScale(3)*[-1 1]]; % range for plotting
return

function PointCloud = ModelToPointCloud(Model,VoxSize,SubSampFactor)
    PointCloud = PointCloudFromf(Model.f,Model.AxLims,VoxSize);
return

function PointCloudBdy = ModelToPointCloudBdy(Model,VoxSize,SubSampFactor)
    PointCloud    = PointCloudFromf(Model.f,Model.AxLims,VoxSize);
    PointCloudBdy = PointCloudBdyFromfAndfGrad(Model.f,Model.fGrad,PointCloud,VoxSize,SubSampFactor);
return

function Model = ModelRotateTranslate(Model,Thetaxyz,Offset)
    RotMtx = AnglesToRotMtx(Thetaxyz);
    Model  = ModelAffineTransform(Model,RotMtx,Offset);
return

function Model = ModelAffineTransform(Model,A,b)
    Model.f      = fAffineTransform(Model.f,A,b);
    Model.fGrad  = fGradAffineTransform(Model.fGrad,A,b);
    Model.AxLims = AxLimsAffineTransform(Model.AxLims,A,b);
return

%====================================================
% Point Cloud 3D geometry stuff
%====================================================
function [GridPts,AxPts] = GetGridPts(NPtsPerAx,AxLength)
    %AxPts = linspace(0,AxLength,NPtsPerAx);
    AxPts = linspace(-AxLength,AxLength,NPtsPerAx);
    [X,Y,Z] = meshgrid(AxPts,AxPts,AxPts);
    GridPts = [X(:),Y(:),Z(:)];
return

function [Cloud,nPtsCloud] = PointCloudFromf(f,AxLim,VoxSize)
% returns point cloud of 1-sublevel set of f, sampled over a box specified
% by AxLim, with granularity VoxSize
    xMin=AxLim(1);xMax=AxLim(2);yMin=AxLim(3);yMax=AxLim(4);zMin=AxLim(5);zMax=AxLim(6);
    [X,Y,Z] = meshgrid([xMin:VoxSize:xMax],[yMin:VoxSize:yMax],[zMin:VoxSize:zMax]);
    GridPts = [X(:),Y(:),Z(:)];
    nPts    = size(GridPts,1);
    InOrOut = zeros(nPts,1);
    for i=1:nPts
        InOrOut(i) = ( f(GridPts(i,:)') <= 1);
    end
    IndexCloudPts = find(InOrOut > 0);
    Cloud         = GridPts(IndexCloudPts,:);
    nPtsCloud     = length(IndexCloudPts);
return

function [CloudBdy,nPtsCloudBdy] = PointCloudBdyFromfAndfGrad(f,fGrad,Cloud,VoxSize,SubSampFactor)
% returns boundary points of point cloud of 1-sublevel set of f,
% assumed to be differentiable with gradient fGrad, also assumed
% be computed with granularity VoxSize, and will be downsampled by factor SubSampFactor
    if nargin < 4, VoxSize = 0.1;end
    if nargin < 5, SubSampFactor = 1; end
    n       = size(Cloud,1);
    InOrOut = zeros(n,1);
    for i=1:SubSampFactor:n
        xi = Cloud(i,:)';
        InOrOut(i) = ( abs(f(xi)-1)  <=  norm(fGrad(xi))*VoxSize );% point on shell
    end
    IndexBdyPts = find( InOrOut > 0 );
    CloudBdy    = Cloud(IndexBdyPts,:);
    nPtsCloudBdy= size(CloudBdy,1);
return

function f = fAffineTransform(f,A,b)
% update f so that its 1-sublevel set S is transformed to A*S+b
    AInv = inv(A);
    f = @(x) f(AInv*(x-b));
return

function fGrad = fGradAffineTransform(fGrad,A,b)
% update fGrad to correspond to the f whose 1-sublevel set S is transformed to A*S+b
    AInv = inv(A);
    fGrad = @(x) AInv'*fGrad(AInv*(x-b));
return

function AxLims = AxLimsAffineTransform(AxLims,A,b)
    Vertices = AxLimsToVertices(AxLims);
    Vertices = PointCloudAffineTransform(Vertices,A,b);
    AxLims   = AxLimsFromVertices(Vertices);
return

function AxLims = AxLimsFromVertices(Vertices)
% computes box around set of Vertices sorted row-wise [x1';x2';...;xN'];
    Dimensions = size(Vertices,2);
    AxLims = zeros(1,2*Dimensions);
    for i=1:Dimensions
        AxLims(2*i)  = max(Vertices(:,i));
        AxLims(2*i-1)= min(Vertices(:,i));
    end
return

function Vertices = AxLimsToVertices(AxLims)
    Vertices = GenerateVertices([],0,AxLims);
return

function V = GenerateVertices(V,i,AxLims)
% computes Vertices sorted row-wise [x1';x2';...;xN'] from Matlab axis limits AxLims,
% via recursively doubling up the coordinates matrix, backwards from N to 1,
% adding new coordinate column each step.
    n = length(AxLims)/2;
    i = i+1;
    if i <= n
        V = [AxLims(2*(n-i+1))  *ones(2^(i-1),1),V;
             AxLims(2*(n-i+1)-1)*ones(2^(i-1),1),V];
        V = GenerateVertices(V,i,AxLims);
    else
        V = V;
    end
return

function Cloud = PointCloudAffineTransform(Cloud,A,b)
% Apply A*x+b to each point in Cloud
% Assumes points stored in Cloud row-wise [x1';x2';...;xN'];
    nPtsCloud = size(Cloud,1);
    Cloud     = Cloud*A' + repmat(b(:)',nPtsCloud,1);
return

function Cloud = PointCloudRotateTranslate(Cloud,Thetaxyz,Offset)
% Apply rotation (Thetaxyz) and Offset to each point in Cloud
% Assumes points stored in Cloud row-wise [x1';x2';...;xN'];
    RotMtx = AnglesToRotMtx(Thetaxyz);
    Cloud = PointCloudAffineTransform(Cloud,RotMtx,Offset(:));
return

function [RotMtx,Rx,Ry,Rz] = AnglesToRotMtx(Thetaxyz)
% Rotation about x, y, and z axes - in that order!
    cx = cos(Thetaxyz(1)); sx = sin(Thetaxyz(1));
    cy = cos(Thetaxyz(2)); sy = sin(Thetaxyz(2));
    cz = cos(Thetaxyz(3)); sz = sin(Thetaxyz(3));
    Rx  = [1    0   0;
           0    cx -sx;
           0    sx  cx];
    Ry  = [cy   0   sy;
           0    1   0;
          -sy   0   cy];
    Rz  = [cz  -sz  0;
           sz   cz  0;
           0    0   1];
    RotMtx = Rz*Ry*Rx;
return

function PlotCloud(CloudGridPts,Symbol,AxPts)
    plot3(CloudGridPts(:,1),CloudGridPts(:,2),CloudGridPts(:,3),Symbol);
    AxMin = min(AxPts); AxMax = max(AxPts);
    axis([AxMin,AxMax,AxMin,AxMax,AxMin,AxMax])
    grid on
return

function Points = GetPointsOnUnitSphere1000(N)
% get evenly distributed 1000 points on UNIT sphere
% taken off the web
    if nargin <1
        N=14;
    end
    Beta = 0.5*pi/N;
    Points = [];
    % line segment length
    A = 2*sin(Beta/2);
    % endcap
    Points = [Points;[0 0 1]];
    Points = [Points;[0 0 -1]];
    % rings
    for i=1:N
      R = sin(i*Beta);
      Z = cos(i*Beta);
      M = round(R*2*pi/A);
      for j=0:M-1
          Alpha = j/M * 2 * pi;
          X = cos(Alpha)*R;
          Y = sin(Alpha)*R;
          Points = [Points;[X Y Z]];
          if i~=N
              Points = [Points;[X Y -Z]];
          end
      end
    end
return

%====================================================
% Optimization functions: CPLEX and ADMM
%====================================================
function xBmWtsStar = OptimizeBeamsCPLEX(AAll,dMin,dMax);
% solve using CPLEX
%        minimize   sum pos(d - dmax)  + sum pos(dmin - d)
%        such that  A x =  d
%                     x >= 0
% where pos(x) is the positive part of x, taken componentwise
%         pos(x) = max(x,0)
%
    NVoxAll  = size(AAll,1)
    NBeams   = size(AAll,2)
    INVoxAll = speye(NVoxAll);
    ZNVoxAll = sparse(NVoxAll,NVoxAll);
    ZNVoxAllNBeams = sparse(NVoxAll,NBeams);
    AEq = [AAll, -INVoxAll, ZNVoxAll];
    AEq = sparse(AEq);
    bEq = [zeros(NVoxAll,1)];
    AIneq = [ZNVoxAllNBeams, -INVoxAll, -INVoxAll;
             ZNVoxAllNBeams,  INVoxAll, -INVoxAll];
    AIneq = sparse(AIneq);
    bIneq = [-dMin;dMax];
    lb = zeros(NBeams+NVoxAll+NVoxAll,1);
    ub = [300*ones(NBeams,1);1e6*ones(2*NVoxAll,1)];
    cLP = [zeros(NBeams,1);zeros(NVoxAll,1);ones(NVoxAll,1)];
    disp('Solving optimization with CPLEX ...')
    tic
    options = cplexoptimset('Simplex','on');
    [xStar,Obj,exitflag,output,lambda] = ...
    cplexlp(cLP',AIneq,bIneq,AEq,bEq,lb,ub,[],options); %, x0);
    exitflag = exitflag
    xBmWtsStar = xStar(1:NBeams);
    fstar = Obj
    toc
return

function xBmWtsStar = OptimizeBeamsADMM(AAll,dMin,dMax);
% solve using ADMM following Stanford EE364b final exam problem (Boyd&Chu)
%        minimize   sum pos(d - dmax)  + sum pos(dmin - d)
%        such that  A x =  d
%                     x >= 0
% where pos(x) is the positive part of x, taken componentwise
%         pos(x) = max(x,0)
%
    tic
    disp(' ')
    NVoxAll  = size(AAll,1)
    NBeams   = size(AAll,2)
    disp('doing ADMM as in EE364b 2011 Final (Stephen Boyd, Eric Chu)')
    disp('computing pseudo-inverse of [A;I]...')
    tic
    AAllI    = [AAll;eye(NBeams)];
    %AAllIPinv = pinv(AAllI);
    Delta = pinv(speye(NBeams)+AAll'*AAll);
    save('Delta','Delta');
    %load Delta
    toc
    ud  = zeros(NVoxAll,1);
    ux  = zeros(NBeams,1);
    dose = zeros(NVoxAll,1);
    xBm  = zeros(NBeams,1);
    z    = zeros(NBeams,1);
    zOld = z;
    disp('doing ADMM iterations')
    MaxIters = 300
    rho     = 1.25;%0.5
    mu      = 10;%1.5
    tauIncr = 2;%1.1
    tauDecr = 1/tauIncr;
    rNorm = zeros(MaxIters,1);
    sNorm = zeros(MaxIters,1);
    rhos  = zeros(MaxIters,1);
    for i=1:MaxIters

        % do ADMM update
        AAllzOld = AAll*zOld;
        dose = DoubleHingeProx(dMin,dMax,rho,AAllzOld-(ud/rho));
        xBm  = min(max(z-(ux/rho),0),300);
        z    = Delta*(  AAll'*(dose+(ud/rho))  +  (xBm+(ux/rho))  );
        AAllz= AAll*z;
        ux   = ux + rho*(xBm-z);
        ud   = ud + rho*(dose-AAllz);

        % compute primal and dual residuals for stopping criterion and rho updating
        r        = [dose;xBm] - [AAllz;z];
        s        = rho*([AAllz-AAllzOld;z-zOld]);
        rNorm(i) = norm(r,2);
        sNorm(i) = norm(s,2);

        % update z
        zOld = z;

        % "adaptive" rho update (doesn't always work ==> safer to switch off)
        rhos(i) = rho;
%         if     (rNorm(i) > mu*sNorm(i))
%             rho = tauIncr*rho
%         elseif (rNorm(i) < (1/mu)*sNorm(i))
%             rho = tauDecr*rho
%         end

    end
    disp('done!')
    xBmWtsStar = xBm;
    toc

    figure
    subplot(3,1,1),plot(1:MaxIters,log10(rNorm)),grid,ylabel('log10(rNorm)')
    subplot(3,1,2),plot(1:MaxIters,log10(sNorm)),grid,ylabel('log10(sNorm)')
    subplot(3,1,3),plot(1:MaxIters,log10(rhos)),grid,ylabel('log10(rho)')
return

function xProx = DoubleHingeProx(aa,bb,rho,x)
    if min(bb-aa)<0, error('xMin > xMax!'),end
    xProx = 1/rho + x - max(x-(aa-1/rho),0) + max(x-aa,0) - max(x-bb,0) + max(x-(bb+1/rho),0);
return

function xProx = DoubleHingeProx2(aa,bb,rho,x)
    if min(bb-aa)<0, error('xMin > xMax!'),end
    xTemp = SoftThresh(1/2/rho,x-(aa-1/2/rho)) + aa;
    xProx = SoftThresh(1/2/rho,xTemp-(bb+1/2/rho))+bb;
return

function xST = SoftThresh(k,x)
    if min(k) <= 0, error('k <=0'),end
    xST = max(x-k,0) - max(-x-k,0);
return

function y = DoubleHinge(xMin,xMax,x)
    if min(xMax-xMin)<0, error('xMin > xMax!'),end
    y = max(xMin-x,0) + max(x-xMax,0);
return


%% Setting of the problem
global s
pde = fracLapdata9;
% pde = checkboarddata;
pde.L = 1;
option.maxIt = 4;
option.maxN = 1e6;
option.elemType = 'P1P2';
option.solver = 'mg';
option.gNquadorder = 4;
option.tol = 1e-6;
[node,elem] = squaremesh([-1 1 -1 1],0.5);
bdFlag = setboundary(node,elem,'Dirichlet');

%% s = 0.2
s = 0.2;
femfracLap(node,elem,pde,bdFlag,option);

%% s = 0.4
s = 0.4;
femfracLap(node,elem,pde,bdFlag,option);

%% s = 0.6
s = 0.6;
femfracLap(node,elem,pde,bdFlag,option);

%% s = 0.8
s = 0.8;
femfracLap(node,elem,pde,bdFlag,option);


function [D]=logic2levelset(varargin)

% function [D]=logic2levelset(logicInside,voxelSize,voxelSizeResample)
% ------------------------------------------------------------------------
% This function converts the logic image logicInside to a level set image.
% The level set image intensities define the distance of the voxels to the
% boundaries of the logic image.
%
%
% Change log:
%
% ------------------------------------------------------------------------
%%

switch nargin
    case 1
        logicInside=varargin{1};
        voxelSize=[];
        voxelSizeResample=[];
    case 2
        logicInside=varargin{1};
        voxelSize=varargin{2};
        voxelSizeResample=[];
    case 3
        logicInside=varargin{1};
        voxelSize=varargin{2};
        voxelSizeResample=varargin{3};
end

if isempty(voxelSize)
    voxelSize=ones(1,3);
end

if isempty(voxelSizeResample)
    voxelSizeResample=mean(voxelSize);
end

if numel(voxelSizeResample)==1
    voxelSizeResample=voxelSizeResample*ones(1,3);
end

%%

siz=size(logicInside);
%Resample input image isotropically (isotropic voxels)
if ~max(voxelSize-voxelSizeResample)< max(eps(voxelSize))
    resampleOn=1;
    [logicInside]=imageResample(logicInside,voxelSize,voxelSizeResample);
    logicInside=logicInside>0; %Forces logic and fixes potential NaN's
    scaleFactor=voxelSizeResample(1);
else
    resampleOn=0;
    scaleFactor=voxelSize(1);
end
logicInside=logicInside>0; %Force to be a logic (in case resampling altered it)

%Remove interior from logic
try
    logicOn = bwmorph3(logicInside,'remove'); %New in R2018a
catch
    logicOn=logicRemoveInterior(logicInside); %GIBBON alterative
end

%Do distance transform on isotropic image
D = double(bwdist(logicOn,'euclidean')); %Compute distance
D(logicInside)=-D(logicInside); %Negate distance inside
D=D.*scaleFactor; %Scale by voxel size

if resampleOn==1
    sizNew=size(D); %Size of distance data in resampled state
    [J,I,K]=meshgrid(1:1:siz(2),1:1:siz(1),1:1:siz(3)); %Image coordinate grid of original image
    [X,Y,Z]=im2cart(I,J,K,voxelSize); %Spatial coordinate of original image grid
    [I,J,K]=cart2im(X,Y,Z,voxelSizeResample); %Image coordinates of original grid in resampled image
    I=round(I); J=round(J); K=round(K); %Rounding
    IND=reshape(sub2indn(sizNew,[I(:) J(:) K(:)],1),size(I)); %Convert to linear indices
    D=D(IND); %Override D to be data sampled at original image points
end

%%
% _*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/>.