function [fx,tt]=v_fxrapt(s,fs,mode,q)
%V_FXRAPT RAPT pitch tracker [FX,VUV]=(S,FS,M,Q)
%
% Input:   s(ns)      Speech signal
%          fs         Sample frequency (Hz)
%          mode       'g' will plot a graph [default if no output arguments]
%                     'u' will include unvoiced fames (with fx=NaN)
%          q          stucture with parameter values (e.g. q.f0min=40); see below for a list
%
% Outputs: fx(nframe)     Larynx frequency for each fram,e (or NaN for silent/unvoiced)
%          tt(nframe,3)  Start and end samples of each frame. tt(*,3)=1 at the start of each talk spurt
%
% Plots a graph if no outputs are specified showing lag candidates and selected path
%

% Bugs/Suggestions:
%   (1) Include backward DP pass and output the true cost for each candidate.
%   (2) Add an extra state to distinguish between voiceless and silent
%   (3) N-best DP to allow longer term penalties (e.g. for frequent pitch doubling/halving)

% The algorithm is taken from [1] with the following differences:
%
%      (a)  the factor AFACT which in the Talkin algorithm corresponds roughly
%           to the absolute level of harmonic noise in the correlation window. This value
%           is here calculated as the maximum of three figures:
%                   (i) an absolute floor set by p.absnoise
%                  (ii) a multiple of the peak signal set by p.signoise
%                 (iii) a multiple of the noise floor set by p.relnoise
%      (b) The LPC used in calculating the Itakura distance uses a Hamming window rather than
%          a Hanning window.
%
% A C implementation of this algorithm by Derek Lin and David Talkin is included as  "get_f0.c"
% in the esps.zip package available from http://www.speech.kth.se/esps/esps.zip under the BSD
% license.
%
% Refs:
%      [1]   D. Talkin, "A Robust Algorithm for Pitch Tracking (RAPT)"
%            in "Speech Coding & Synthesis", W B Kleijn, K K Paliwal eds,
%            Elsevier ISBN 0444821694, 1995

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

s=s(:); % force s to be a column
if nargin<4
    q=[];
    if nargin<3
        mode=' ';
    end
end
doback=0;   % don't do backwards DP for now

% set default parameters

p0.f0min=50;           % Min F0 (Hz)
p0.f0max=500;          % Max F0 (Hz)
p0.tframe=0.01;        % frame size (s)
p0.tlpw=0.005;         % low pass filter window size (s)
p0.tcorw=0.0075;       % correlation window size (s)
p0.candtr=0.3;         % minimum peak in NCCF
p0.lagwt=0.3;          % linear lag taper factor
p0.freqwt=0.02;        % cost factor for F0 change
p0.vtranc=0.005;       % fixed voice-state transition cost
p0.vtrac=0.5;          % delta amplitude modulated transition cost
p0.vtrsc=0.5;          % delta spectrum modulated transition cost
p0.vobias=0.0;         % bias to encourage voiced hypotheses
p0.doublec=0.35;       % cost of exact doubling or halving
p0.absnoise=0;         % absolute rms noise level
p0.relnoise=2;         % rms noise level relative to noise floor
p0.signoise=0.001;     % ratio of peak signal rms to noise floor (0.001 = 60dB)
p0.ncands=20;          % max hypotheses at each frame
p0.trms=0.03;                      % window length for rms measurement
p0.dtrms=0.02;                     % window spacing for rms measurement
p0.preemph=-7000;                  % s-plane position of preemphasis zero
p0.nfullag=7;                      % number of full lags to try (must be odd)
p=v_paramsetch(p0,q);

% redefine commonly used parameters

candtr=p.candtr;          % minimum peak in NCCF                      [0.3]
vtranc=p.vtranc;          % fixed voice-state transition cost         [0.005]
vtrac=p.vtrac;            % delta amplitude modulated transition cost [0.5]
vtrsc=p.vtrsc;            % delta spectrum modulated transition cost  [0.5]
vobias=p.vobias;          % bias to encourage voiced hypotheses       [0.0]
doublec=p.doublec;        % cost of exact doubling or halving         [0.35]
ncands=p.ncands;          % max hypotheses at each frame              [20]
nfullag=p.nfullag;        % number of full lags to try (must be odd)  [7]

% derived parameters (mostly dependent on sample rate fs)

krms=round(p.trms*fs);            % window length for rms measurement
kdrms=round(p.dtrms*fs);          % window spacing for rms measurement
% rmswin=hanning(krms).^2;
rmswin=(0.5-0.5*cos(2*pi*(1:krms)'/(krms+1))).^2; % squared Hanning window
kdsmp=round(0.25*fs/p.f0max);
hlpw=round(p.tlpw*fs/2);          % force window to be an odd length
blp=sinc((-hlpw:hlpw)/kdsmp).*hamming(2*hlpw+1).';
fsd=fs/kdsmp;
kframed=round(fsd*p.tframe);      % downsampled frame length
kframe=kframed*kdsmp;           % frame increment at full rate
rmsix=(1:krms)+floor((kdrms-kframe)/2); % rms index according to Talkin; better=(1:krms)+floor((kdrms-krms+1)/2)
minlag=ceil(fsd/p.f0max);
maxlag=round(fsd/p.f0min);        % use round() only because that is what Talkin does
kcorwd=round(fsd*p.tcorw);        % downsampled correlation window
kcorw=kcorwd*kdsmp;             % full rate correlation window
spoff=max(hlpw-floor(kdsmp/2),1+kdrms-rmsix(1)-kframe);  % offset for first speech frame at full rate
sfoff=spoff-hlpw+floor(kdsmp/2); % offset for downsampling filter
sfi=1:kcorwd;                   % initial decimated correlation window index array
sfhi=1:kcorw;                   % initial correlation window index array
sfj=1:kcorwd+maxlag;
sfmi=repmat((minlag:maxlag)',1,kcorwd)+repmat(sfi,maxlag-minlag+1,1);
lagoff=(minlag-1)*kdsmp;        % lag offset when converting to high sample rate
beta=p.lagwt*p.f0min/fs;            % bias towards low lags
log2=log(2);
lpcord=2+round(fs/1000);        % lpc order for itakura distance
hnfullag=floor(nfullag/2);
jumprat=exp((doublec+log2)/2);  % lag ratio at which octave jump cost is lowest
ssq=s.^2;
csssq=cumsum(ssq);
sqrt(min(csssq(kcorw+1:end)-csssq(1:end-kcorw))/kcorw);
afact=max([p.absnoise^2,max(ssq)*p.signoise^2,min(csssq(kcorw+1:end)-csssq(1:end-kcorw))*(p.relnoise/kcorw)^2])^2*kcorw^2;

% downsample signal to approx 2 kHz to speed up autocorrelation calculation
% kdsmp is the downsample factor

sf=filter(blp/sum(blp),1,s(sfoff+1:end));
sp=filter([1 exp(p.preemph/fs)],1,s); % preemphasised speech for LPC calculation
sf(1:length(blp)-1)=[];         % remove startup transient
sf=sf(1:kdsmp:end);             % downsample to =~2kHz
nsf=length(sf);                 % length of downsampled speech
ns=length(s);                   % length of full rate speech

% Calculate the frame limit to ensure we don't run off the end of the speech or decimated speech:
%   (a) For decimated autocorrelation when calculating sff():  (nframe-1)*kframed+kcorwd+maxlag <= nsf
%   (b) For full rate autocorrelation when calculating sfh():  max(fho)+kcorw+maxlag*kdsamp+hnfllag <= ns
%   (c) For rms ratio window when calculating rr            :  max(fho)+rmsix(end) <= ns
% where max(fho) = (nframe-1)*kframe + spoff

nframe=floor(1+min((nsf-kcorwd-maxlag)/kframed,(ns-spoff-max(kcorw-maxlag*kdsmp-hnfullag,rmsix(end)))/kframe));

% now search for autocorrelation peaks in the downsampled signal

cost=zeros(nframe,ncands);      % cumulative cost
prev=zeros(nframe,ncands);      % traceback pointer
mcands=zeros(nframe,1);         % number of actual candidates excluding voiceless
lagval=repmat(NaN,nframe,ncands-1);    % lag of each voiced candidate
tv=zeros(nframe,3);             % diagnostics: 1=voiceless cost, 2=min voiced cost, 3:cumulative voiceless-min voiced
if doback
    costms=cell(nframe,1);
end

% Main processing loop for each 10 ms frame

for iframe=1:nframe       % loop for each frame (~10 ms)

    % Find peaks in the normalized autocorrelation of subsampled (2Khz) speech
    % only keep peaks that are > 30% of highest peak

    fho=(iframe-1)*kframe+spoff;
    sff=sf((iframe-1)*kframed+sfj);
    sffdc=mean(sff(sfi));       % mean of initial correlation window length
    sff=sff-sffdc;              % subtract off the mean
    nccfd=normxcor(sff(1:kcorwd),sff(minlag+1:end));
    [ipkd,vpkd]=v_findpeaks(nccfd,'q');

    % Debugging: execute the line below to plot the autocorrelation peaks.
    % v_findpeaks(nccfd,'q'); xlabel(sprintf('Lag = (x+%d)*%g ms',minlag-1,1000*kdsmp/fs)); ylabel('Normalized Cross Correlation'); title (sprintf('Frame %d/%d',iframe,nframe));

    vipkd=[vpkd ipkd];
    vipkd(vpkd<max(vpkd)*candtr,:)=[];          % eliminate peaks that are small
    if size(vipkd,1)
        if size(vipkd,1)>ncands-1
            vipkd=sortrows(vipkd);
            vipkd(1:size(vipkd,1)-ncands+1,:)=[];   % eliminate lowest to leave only ncands-1
        end
        lagcan=round(vipkd(:,2)*kdsmp+lagoff);        % convert the lag candidate values to the full sample rate
        nlcan=length(lagcan);
    else
        nlcan=0;
    end

    % If there are any candidate lag values (nlcan>0) then refine their accuracy at the full sample rate

    if nlcan
        laglist=reshape(repmat(lagcan(:)',nfullag,1)+repmat((-hnfullag:hnfullag)',1,nlcan),nfullag*nlcan,1);
        sfh=s(fho+(1:kcorw+max(lagcan)+hnfullag));
        sfhdc=mean(sfh(sfhi));
        sfh=sfh-sfhdc;
        e0=sum(sfh(sfhi).^2);                     % energy of initial correlation window (only needed to store in tv(:,6)
        lagl2=repmat(lagcan(:)',nfullag+kcorw-1,1)+repmat((1-hnfullag:hnfullag+kcorw)',1,nlcan);
        nccf=normxcor(sfh(1:kcorw),sfh(lagl2),afact);

        [maxcc,maxcci]=max(nccf,[],1);
        vipk=[maxcc(:) lagcan(:)+maxcci(:)-hnfullag-1];
        vipk=vipk(:,[1 2 2]);
        maxccj=maxcci(:)'+nfullag*(0:nlcan-1);    % vector index into nccf array
        msk=mod(maxcci,nfullag-1)~=1 & 2*nccf(maxccj)-nccf(mod(maxccj-2,nfullag*nlcan)+1)-nccf(mod(maxccj,nfullag*nlcan)+1)>0;  % don't do quadratic interpolation for the end ones
        if any(msk)
            maxccj=maxccj(msk);
            vipk(msk,3)=vipk(msk,3)+(nccf(maxccj+1)-nccf(maxccj-1))'./(2*(2*nccf(maxccj)-nccf(maxccj-1)-nccf(maxccj+1)))';
        end
        vipk(maxcc<max(maxcc)*candtr,:)=[];          % eliminate peaks that are small
        if size(vipk,1)>ncands-1
            vipk=sortrows(vipk);
            vipk(1:size(vipk,1)-ncands+1,:)=[];   % eliminate lowest to leave only ncands-1
        end

        % vipk(:,1) has NCCF value, vipk(:,2) has integer peak position, vipk(:,3) has refined peak position

        mc=size(vipk,1);
    else
        mc=0;
    end

    % We now have mc lag candidates at the full sample rate

    mc1=mc+1;               % total number of candidates including "unvoiced" possibility
    mcands(iframe)=mc;      % save number of lag candidates (needed for pitch consistency cost calculation)
    if mc
        lagval(iframe,1:mc)=vipk(:,3)';
        cost(iframe,1)=vobias+max(vipk(:,1));   % voiceless cost
        cost(iframe,2:mc1)=1-vipk(:,1)'.*(1-beta*vipk(:,3)');   % local voiced costs
        tv(iframe,2)=min(cost(iframe,2:mc1));
    else
        cost(iframe,1)=vobias;          % if no lag candidates (mc=0), then the voiceless case is the only possibility
    end
    tv(iframe,1)=cost(iframe,1);
    if iframe>1                         % if it is not the first frame, then calculate pitch consistency and v/uv transition costs
        mcp=mcands(iframe-1);
        costm=zeros(mcp+1,mc1);         % cost matrix: rows and cols correspond to candidates in previous and current frames (incl voiceless)

        % if both frames have at least one lag candidate, then calculate a pitch consistency cost

        if mc*mcp
            lrat=abs(log(repmat(lagval(iframe,1:mc),mcp,1)./repmat(lagval(iframe-1,1:mcp)',1,mc)));
            costm(2:end,2:end)=p.freqwt*min(lrat,doublec+abs(lrat-log2));  % allow pitch doubling/halving
        end

        % if either frame has a lag candidate, then calculate the cost of voiced/voiceless transition and vice versa

        if mc+mcp
            rr=sqrt((rmswin'*s(fho+rmsix).^2)/(rmswin'*s(fho+rmsix-kdrms).^2)); % amplitude "gradient"
            ss=0.2/(v_distitar(v_lpcauto(sp(fho+rmsix),lpcord),v_lpcauto(sp(fho+rmsix-kdrms),lpcord),'e')-0.8);   % Spectral stationarity: note: Talkin uses Hanning instead of Hamming windows for LPC
            costm(1,2:end)= vtranc+vtrsc*ss+vtrac/rr;   % voiceless -> voiced cost
            costm(2:end,1)= vtranc+vtrsc*ss+vtrac*rr;
            tv(iframe,4:5)=[costm(1,mc1) costm(mcp+1,1)];
        end
        costm=costm+repmat(cost(iframe-1,1:mcp+1)',1,mc1);  % add in cumulative costs
        [costi,previ]=min(costm,[],1);
        cost(iframe,1:mc1)=cost(iframe,1:mc1)+costi;
        prev(iframe,1:mc1)=previ;
    else                            % first ever frame
        costm=zeros(1,mc1); % create a cost matrix in case doing a backward recursion
    end
    if mc
        tv(iframe,3)=cost(iframe,1)-min(cost(iframe,2:mc1));
        tv(iframe,6)=5*log10(e0*e0/afact);
    end
    if doback
        costms{iframe}=costm; % need to add repmatted cost into this
    end
end

% now do traceback

best=zeros(nframe,1);
[cbest,best(nframe)]=min(cost(nframe,1:mcands(nframe)+1));
for i=nframe:-1:2
    best(i-1)=prev(i,best(i));
end
vix=find(best>1);
fx=repmat(NaN,nframe,1);                        % unvoiced frames will be NaN
fx(vix)=fs*lagval(vix+nframe*(best(vix)-2)).^(-1); % leave as NaN if unvoiced
tt=zeros(nframe,3);
tt(:,1)=(1:nframe)'*kframe+spoff;       % find frame times
tt(:,2)=tt(:,1)+kframe-1;
jratm=(jumprat+1/jumprat)/2;
tt(2:end,3)=abs(fx(2:end)./fx(1:end-1)-jratm)>jumprat-jratm;    % new spurt if frequency ratio is outside (1/jumprat,jumprat)
tt(1,3)=1;           % first frame always starts a spurt
tt(1+find(isnan(fx(1:end-1))),3)=1; % NaN always forces a new spurt

% plot results if there are no output arguments of if the 'g' mode option is specified

if ~nargout || any(mode=='g')
    tf=spoff+(0:nframe-1)'*kframe;      % one sample before start of each frame
    blag=repmat(NaN,nframe,1);                        % unvoiced frames will be NaN
    blag(vix)=lagval(vix+nframe*(best(vix)-2)); % leave as NaN if unvoiced
    ts=(1:ns)/fs;                       % time scale for speech samples
    tsa=[1:tf(1) tf(end)+kframe+1:ns];  % indexes for unprocessed speech [-1 term is an error methinks]
    sup=repmat(NaN,ns,1);               % unprocessed speech - plot in black
    sup(tsa)=s(tsa);
    sv=reshape(s(tf(1)+1:tf(end)+kframe),kframe,nframe);               % processed speech
    su=sv;
    su(:,best>1)=NaN;                   % delete all voiced samples
    sv(:,best==1)=NaN;                  % delete all unvoiced samples
    tsuv=(tf(1)+1:tf(end)+kframe)/fs;
    su=su(:);
    sv=sv(:);
    ax=zeros(2,1);
    ax(1)=subplot(211);
    plot(ts,sup,'-k',tsuv,su,'r-',tsuv,sv,'b-');
    title('Speech');
    ax(2)=subplot(212);
    plot((tf+(kframe+1)/2)/fs,lagval*1000/fs,'xr',(tf+(kframe+1)/2)/fs,blag*1000/fs,'-b')
    xlabel('Time (s)');
    ylabel('Period (ms)');
    title('Lag Candidates');
    linkaxes(ax,'x');
end
if ~any(mode=='u')
    tt(isnan(fx),:)=[];    % remove NaN spurts
    fx(isnan(fx),:)=[];
end

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

function v=normxcor(x,y,d)
% Calculate the normalized cross correlation of column vectors x and y
% we can calculate this in two ways but fft is much faster even for nx small
% We must have nx<=ny and the output length is ny-nx+1
% note that this routine does not do mean subtraction even though this is normally a good idea
% if y is a matrix, we correlate with each column
% d is a constant added onto the normalization factor
% v(j)=x'*yj/sqrt(d + x'*x * yj'*yj) where yj=y(j:j+nx-1) for j=1:ny-nx+1

if nargin<3
    d=0;
end
nx=length(x);
[ny,my]=size(y);
nv=1+ny-nx;
if nx>ny
    error('second argument is shorter than the first');
end

nf=pow2(nextpow2(ny));
w=v_irfft(repmat(conj(v_rfft(x,nf,1)),1,my).*v_rfft(y,nf,1));
s=zeros(ny+1,my);
s(2:end,:)=cumsum(y.^2,1);
v=w(1:nv,:)./sqrt(d+(x'*x).*(s(nx+1:end,:)-s(1:end-nx,:)));


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nick.battista@unc.edu
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "Hill+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the target point positions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function targets = update_Target_Point_Positions(dt,current_time,targets)

IDs = targets(:,1);                 % Stores Lag-Pt IDs in col vector
xPts= targets(:,2);                 % Original x-Values of x-Target Pts.
yPts= targets(:,3);                 % Original y-Values of y-Target Pts.
kStiffs = targets(:,4);             % Stores Target Stiffnesses

N_target = length(targets(:,1));    %Gives total number of target pts!

% Coefficients for Polynomial Phase-Interpolation
a = 2.739726027397260;  % y1(t) = at^2
b = 2.739726027397260;  % y3(t) = -b(t-1)^2+1
c = -2.029426686960933; % y2(t) = ct^3 + dt^2 + gt + h
d = 3.044140030441400;
g = -0.015220700152207;
h = 0.000253678335870;

% Period Info
tP1 = 0.025;                        % Contraction time
tP2 = 0.025;                        % Expansion time
period = tP1+tP2;                   % Period
t = rem(current_time,period);       % Current time in simulation ( 'modular arithmetic to get time in period')

% Read In Pts!
[xP1,yP1,xP2,yP2] = read_File_In('All_Positions.txt');

for i=1:N_target                    % Loops over all target points!

    if (t <= tP1)

			%PHASE 1 --> PHASE 2

			tprev = 0.0;
			t1 = 0.1*tP1;
			t2 = 0.9*tP1;
			if (t<t1) 							%For Polynomial Phase Interp.
				g1 = a*power((t/tP1),2);
            elseif ((t>=t1)&&(t<t2))
				g1 = c*power((t/tP1),3) + d*power((t/tP1),2) + g*(t/tP1) + h;
            elseif (t>=t2)
				g1 = -b*power(((t/tP1) - 1),2) + 1;
            end

			xPts(IDs(i)) = xP1(i) + g1*( xP2(i) - xP1(i) );
			yPts(IDs(i)) = yP1(i) + g1*( yP2(i) - yP1(i) );

    elseif ((t>tP1)&&(t<=(tP1+tP2)))

			%PHASE 2 --> PHASE 1

			tprev = tP1;
			t1 = 0.1*tP2 + tP1;
			t2 = 0.9*tP2 + tP1;
			if (t<t1) 							%//For Polynomial Phase Interp.
				g2 = a*power( ( (t-tprev)/tP2) ,2);
            elseif ((t>=t1)&&(t<t2))
				g2 = c*power( ( (t-tprev)/tP2) ,3) + d*power( ((t-tprev)/tP2) ,2) + g*( (t-tprev)/tP2) + h;
            elseif (t>=t2)
				g2 = -b*power( (( (t-tprev)/tP2) - 1) ,2) + 1;
            end

			xPts(IDs(i)) = xP2(i) + g2*( xP1(i) - xP2(i) );
			yPts(IDs(i)) = yP2(i) + g2*( yP1(i) - yP2(i) );

    end

    targets(IDs(i),2) = xPts(IDs(i)); % Store new xVals
    targets(IDs(i),3) = yPts(IDs(i)); % Store new yVals

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: reads in info from file that contains probability distributions
% (rows) for each game of Bingo w/ N players (columns)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x1,y1,x2,y2] = read_File_In(file_name)

filename = file_name;  %Name of file to read in

fileID = fopen(filename);

    % Read in the file, use 'CollectOutput' to gather all similar data together
    % and 'CommentStyle' to to end and be able to skip lines in file.
    C = textscan(fileID,'%f %f %f %f','CollectOutput',1);

fclose(fileID);        %Close the data file.

mat_info = C{1};   %Stores all read in data

%Store all elements in matrix
mat = mat_info(1:end,1:end);

x1 =  mat(:,1); %store regular bingo expectation values
y1 =  mat(:,2); %store inner bingo expectation values
x2 =  mat(:,3); %store outer bingo expectation values
y2 =  mat(:,4); %store cover all bingo expectation values


function [err, out_fn] = Norm (InBrik, N_runs, run, run_norm, run_mask, foutid_log)
%
%   [err, norm_fn] = Norm(InBrik, N_runs, run)
%
%Purpose:
%
%   Normalize the original dataset so that the beta values in 3dDeconvolve would be automatically percent signal change.
%
%Input Parameters:
%
%
%
%Output Parameters:
%   err : 0 No Problem
%       : 1  Problems
%
%
%
%Key Terms:
%
%More Info :
%
%
%
%
%     Author : Gang Chen
%     Date : Mon Oct 20 11:26:38 EDT 2003
%     SSCC/NIMH/ National Institutes of Health, Bethesda Maryland

%Define the function name for easy referencing
FuncName = 'Norm';

%Debug Flag
DBG = 1;

%initailize return variables
err = 1;

[InBrikPrefix, InBrikView, InBrikExt] =  AfniPrefix (InBrik);
rm_cmmd = 'rm -f ';

%Originally 3dClipLevel was used to generate a cliplevel for each run,
%then a step function chops off those voxel outside of the brain.
%But due to the concern of losing the sensitive amygdala and orbitofrontal cortex,
%it might be better to run 3dAutomask, which is roughly equivalent to
%3dClipLevel but would protect amygdala and orbitofrontal cortex regions
%from being masked.

%create command for executing 3dAutomask
if (run_mask.do == 1),
   automask_fn = sprintf('%s_msk', InBrikPrefix);
   automask = sprintf('3dAutomask -dilate %i -prefix %s %s%s', run_mask.N_dilate, automask_fn, InBrikPrefix, InBrikView);
   fprintf(2, '\nRunning: %s\n', automask);
   fprintf(foutid_log, '%s\n', automask);
   [s,w] = system(automask);
   if (s),
      fprintf(2,'Error during 3dAutomask: \n\n%s\n\n',w);
      while (1); fprintf(2,'Halted: Ctrl+c to exit'); pause; end
   end
	rm_cmmd = sprintf('%s %s%s%s', rm_cmmd, automask_fn, InBrikView, InBrikExt);

	%If no normalization, do masking
   if (run_norm == 0),
	   out_fn = sprintf('%s_mask', InBrikPrefix);
      mask_signal = sprintf('3dcalc -fscale -a  %s%s  \\\n', InBrikPrefix, InBrikView);
 	   mask_signal = sprintf('%s    -b %s%s \\\n', mask_signal, automask_fn, InBrikView);
	   mask_signal = sprintf('%s    -expr  "a*b" \\\n', mask_signal);
      mask_signal = sprintf('%s    -prefix %s', mask_signal, out_fn);
	   fprintf(2, '\nRunning: %s\n', mask_signal);
	   fprintf(foutid_log, '%s\n', mask_signal);
	   [s,w] = system(mask_signal);
      if (s),
         fprintf(2,'Error during 3dcalc: \n\n%s\n\n',w);
         while (1); fprintf(2,'Halted: Ctrl+c to exit'); pause; end
      end
   end
	rm_cmmd = sprintf('%s %s%s%s', rm_cmmd, automask_fn, InBrikView, InBrikExt);
end

if (run_norm == 1),   %for normalization
   out_fn = sprintf('%s_norm', InBrikPrefix);
   concat = sprintf('3dTcat -prefix %s ', out_fn);

   for (i=1:1:N_runs),

   %store each mean dataset in a temporary file, and delete it before leaving this function.
      mean_fn = sprintf('%s_mean%i', InBrikPrefix, i);
	   get_mean = sprintf('3dTstat -mean -prefix %s %s%s''[%i..%i]''', mean_fn, InBrikPrefix, InBrikView, run(i).first, run(i).last);
      fprintf(2, '\nRunning: %s\n', get_mean);
	   fprintf(foutid_log, '%s\n', get_mean);
	   [s, w] = system(get_mean);
	   if (s),
         fprintf(2,'Error at running 3dTstat: \n\n\n%s\n', w);
         while (1); fprintf(2,'Halted: Ctrl+c to exit');pause; end
      end

	%normalizing
%	norm_signal = sprintf('3dcalc -datum float -a  %s%s''[%i..%i]''  \\\n', InBrikPrefix, InBrikView, run(i).first, run(i).last);
	   norm_signal = sprintf('3dcalc -fscale -a  %s%s''[%i..%i]''  \\\n', InBrikPrefix, InBrikView, run(i).first, run(i).last);
   	norm_signal = sprintf('%s    -b %s%s \\\n', norm_signal, mean_fn, InBrikView);
		if (run_mask.do == 1),
		   norm_signal = sprintf('%s    -c %s%s \\\n', norm_signal, automask_fn,InBrikView);
		%	norm_signal = sprintf('%s    -expr  "(a/b*100)*step(b-clip_level)" \\\n', norm_signal);
   	   norm_signal = sprintf('%s    -expr  "(a/b*100)*c" \\\n', norm_signal);
		else norm_signal = sprintf('%s    -expr  "a/b*100" \\\n', norm_signal);
		end
	   norm_out_fn = sprintf('%s_norm%i', InBrikPrefix, i);
   	norm_signal = sprintf('%s    -prefix %s', norm_signal, norm_out_fn);
		fprintf(2, '\nRunning: %s\n', norm_signal);
	   fprintf(foutid_log, '%s\n', norm_signal);
	   [s,w] = system(norm_signal);
      if (s),
         fprintf(2,'Error during 3dcalc: \n\n%s\n\n',w);
         while (1); fprintf(2,'Halted: Ctrl+c to exit'); pause; end
      end

	%Concatenate the above files together
	   concat = sprintf('%s %s%s%s', concat, norm_out_fn, InBrikView, InBrikExt);

	%create remove command for deleting those individual percent signal change and mean file for each run.
	   rm_cmmd = sprintf('%s %s%s%s %s%s%s', rm_cmmd, norm_out_fn, InBrikView, InBrikExt, mean_fn, InBrikView, InBrikExt);

   end

%Concatenate all runs back together
   fprintf(2, '\nRunning: %s\n', concat);
   fprintf(foutid_log, '%s\n', concat);
   [s,w] = system(concat);
   if (s),
      fprintf(2,'Error during running 3dTcat: \n\n%s\n\n',w);
      while (1); fprintf(2,'Halted: Ctrl+c to exit'); pause; end
   end
end

%Don't forget to remove those temporary files above.
fprintf(2, '\nRemoving: %s\n', rm_cmmd);
fprintf(foutid_log, '%s\n', rm_cmmd);
[s,w] = system(rm_cmmd);
if (s),
    fprintf(2,'Error during rm: \n\n%s\n\n',w);
    while (1); fprintf(2,'Halted: Ctrl+c to exit'); pause; end
end

err = 0;
return;


function [countV,ktrAll,veAll,kepAll,vpAll] = runDCETofts(planC)
% runDCETofts.m
% Function to generate and write out DCE parameter maps, compute and save statistics to file.
%
% AI 5/26/16
% AI 7/06/16  Updated to provide option to scale AIF using reference region time course.
%
% Note:
% Required parameters are read from user-input .txt file.
% See ReadDCEParameterFile.m for list of valid input parameters.

%% --- Get model parameters --------

indexS = planC{end};

% Read input parameter file
[inputS,filePath] = ReadDCEParameterFile();
outPath = fileparts(filePath);
[paramS,shiftS] = getInputs(inputS);

% Get DCE scan no.
scanListC = unique({planC{indexS.scan}.scanType});
if numel(scanListC)>1
    [series,ok] = listdlg('ListString',scanListC,'Name','Select series','ListSize',[300,250],...
        'PromptString','Select DCE series','SelectionMode','Single');
    if ~ok
        return
    end
    scanNumV = find(strcmp({planC{indexS.scan}.scanType},scanListC{series}));
else
    scanNumV = 1:length(planC{indexS.scan});
end
scanS = planC{indexS.scan}(scanNumV);

%Extract relevant parameters from DICOM header
headerS.manufacturer = scanS(1).scanInfo(1).DICOMHeaders.Manufacturer;
headerS.xSize = scanS(1).scanInfo(1).DICOMHeaders.Rows;
headerS.ySize = scanS(1).scanInfo(1).DICOMHeaders.Columns;
headerS.RepetitionTime = scanS(1).scanInfo(1).DICOMHeaders.RepetitionTime;
tDel = getTDel(planC);
headerS.tDel = double(tDel)/1000 ; % convert time step to seconds from ms
headerS.MagneticFieldStrength = scanS(1).scanInfo(1).DICOMHeaders.MagneticFieldStrength;
headerS.nSlices = size(scanS(1).scanArray,3);
FA = scanS(1).scanInfo(1).DICOMHeaders.FlipAngle;
headerS.FlipAngle = shiftS.FAF*FA; % correct flip angle by multiplicative factor FAF

switch(headerS.manufacturer)
    case 'GE MEDICAL SYSTEMS'
        totalImages = scanS(1).scanInfo(1).DICOMHeaders.ImagesinAcquisition;
        headerS.frames = totalImages/headerS.nSlices;
    case 'Philips Healthcare'
        headerS.frames = scanS(1).scanInfo(1).DICOMHeaders.NumberofTemporalPositions;
end
%Display:
fprintf(['\nVendor: %s\nNo. slices: %d\nNo. frames: %d\nTR(ms): %g\nDel_T(s): %g\n',...
    'Flip angle: %g\nImage Size(rows x cols): %d x %d\nB0: %g\n'],headerS.manufacturer,...
    headerS.nSlices,headerS.frames,headerS.RepetitionTime,headerS.tDel,...
    headerS.FlipAngle,headerS.xSize,headerS.ySize,headerS.MagneticFieldStrength);

%Time shift for ROI time course (user-input)
ROIPointShift = shiftS.TROIPointShift * strcmp(paramS.model,'T') + ...
    shiftS.ETROIPointShift * strcmp(paramS.model,'ET');
shiftS.ROIPointShift = ROIPointShift;

% r1  (Using relaxivities from Pintaske, et. al. Invest Radiol 2006) **
%     Gd-DTPA, 1.5T   =  3.9 L/(mmol x s)
%     Gd-DTPA, 3.0T   =  3.3 L/(mmol x s)
%     Gd-BOPTA (multihance) 1.5T  = 8.1 L/(mmol x s)
%     Gd-BOPTA (multihance) 3.0T = 6.3 L/(mmol x s)
r1 = 3.9*(headerS.MagneticFieldStrength < 2.0)*strcmp(paramS.cAgent,'gd')+ ...
     3.3*(headerS.MagneticFieldStrength > 2.0)*strcmp(paramS.cAgent,'gd') + ...
     8.1*(headerS.MagneticFieldStrength < 2.0)*strcmp(paramS.cAgent,'mh') + ...
     6.3*(headerS.MagneticFieldStrength > 2.0)*strcmp(paramS.cAgent,'mh');

%Vector of sampling times
ind = double(0:(headerS.frames-1));
tsec = ind*headerS.tDel;
tmin = tsec./60;         % ktrans, etc. are in /min.  Parker coefs are in  min or /min.
TR = headerS.RepetitionTime/1000;  % TR from DICOM header is in ms. Change to s.

% Parameter array for the concentrarion calculation:
%    pCalcConc(1) = R1 = relaxivity of contrast agent (units are mmolxs)
%    pCalcConc(2) = TR (s)
%    pCalcConc(3) = flip angle (degrees)
%    pCalcConc(4) = pre-contrast T1 for tissue (s)
pCalcConc = [r1, TR, headerS.FlipAngle, paramS.T10];

%Generate AIF
%-------------temp:--------
coefFile = fullfile(getCERRPath,'PlanAnalysis','DCE-MR analysis','sample_param_and_coeff_files','Pros_Iliac_17pts_Parker_coefs.txt');
%---------------------------
fprintf('\n Generating AIF from Parker Coefficient file...');
AIFP = genAIF(tsec,coefFile,headerS.frames,tmin,paramS,shiftS);
fprintf('\n AIF generation complete.\n');

%If using T1 maps, load T1 maps for all slices into a 3D array
if ~(paramS.T1Map==0)
    T13M = getScanArray(paramS.T1Map,planC);
else
    T13M = [];
end

%% --------- Get ROI mask ---------------
%Identify structures associated with selected scan
structNumV = 1:length(planC{indexS.structures});
assocScansV = getStructureAssociatedScan(structNumV,planC);
structNumV = structNumV(ismember(assocScansV,scanNumV));
structS =  planC{indexS.structures}(structNumV);
emptyStructV = cellfun(@isempty,{structS.rasterSegments}); %Discard empty structures
structS =  structS(~emptyStructV);

%Extract mask
ROImask3M = true(size(scanS(1).scanArray));
structListC = cellfun(@lower,{structS.structureName},'un',0);
%User-selection of structure:
if numel(structListC)>1
    [strSel,ok] = listdlg('ListString',structListC,'Name','Select structure','ListSize',[300,250],...
        'PromptString','Select ROI','SelectionMode','Single');
    if ~ok
        return
    end
else
    strSel = 1;
end
strNum = structNumV(strSel);
rasterSegments = getRasterSegments(strNum,planC);
[temp,maskSlicesV] = rasterToMask(rasterSegments,1, planC);
ROImask3M(:,:,maskSlicesV) = temp;

%% ---- Concatenate DCE images ----------
sliceC = cell(1,maskSlicesV);
for k = 1:numel(maskSlicesV)
    timeSlices = arrayfun(@(x) x.scanArray(:,:,maskSlicesV(k)),scanS,'un',0);
    sliceC{k} = cat(3,timeSlices{:});
end
sliceTimeCourse4M = cat(4,sliceC{:});
%sliceTimeCourse4M = uint16(sliceTimeCourse4M);   %To match Kristen's code

%% ----- Scale AIF using reference region time course if reqd ----
% Get reference region (muscle) mask
useRefRegion = questdlg('Use muscle reference to scale AIF?','Reference region','Yes','No','No') ;
if strcmp(useRefRegion, 'No')
    AIFPScaled = AIFP;
else
    structListC = {structS.structureName};
    [strNum,ok] = listdlg('ListString',structListC,'SelectionMode','Single','Name','Select reference structure');
    if ~ok
        return
    end
    muscleRasterSegS = getRasterSegments(strNum,planC);
    [muscleMaskM,muscleROISlice] = rasterToMask(muscleRasterSegS,1, planC); %Assumed single-slice
    % Scale AIF using reference region time course
    muscleSliceC = arrayfun(@(x) x.scanArray(:,:,muscleROISlice),scanS,'un',0);
    muscleSliceTimeCourse3M  = cat(3,muscleSliceC{:});
    muscleTimeCourse3M = ROIMaskedSlices(muscleSliceTimeCourse3M,muscleMaskM);
    AIFPScaled = refRegionScale(AIFP,inputS,shiftS,filePath,headerS,muscleTimeCourse3M,muscleMaskM);
end

%% ---- Get parameter maps ----------
getVals = 'Yes';
nRuns = 1;
while(strcmp(getVals,'Yes'))
    if nRuns~=1                   %Runs with value from parameter file if checkShift='n' the first time
        shiftS.checkShift = 'y';  %Set checkShift to 'y' if user chooses to run again with different shift
        [countV,ktrAll,veAll,kepAll,vpAll,dceRsqAll,UserInShift,UserInBase] = getDCEToftsParams(paramS,shiftS,headerS.xSize,headerS.ySize,maskSlicesV,sliceTimeCourse4M,pCalcConc,...
            ROImask3M,T13M,tmin,AIFPScaled,outFolder);
    else
        [countV,ktrAll,veAll,kepAll,vpAll,dceRsqAll,UserInShift,UserInBase,outFolder] = getDCEToftsParams(paramS,shiftS,headerS,maskSlicesV,sliceTimeCourse4M,pCalcConc,...
            ROImask3M,T13M,tmin,AIFPScaled,outPath);
    end
    getVals = questdlg(sprintf('Current shift = %d.\n Run again with different shift?',UserInShift),...
        'Change shift?','Yes','No','No');
    nRuns = nRuns + 1;
end

%% ---Write user-input shifts to param file-------
fid = fopen(filePath);
inputC = textscan(fid, '%s %s %n','endofline','\r\n');
fclose(fid);
fieldsC = fieldnames(paramS);
if strcmp(paramS.model,'T') %Tofts
    shiftIdx = strcmp(fieldsC,'TROIPointShift');
else  %Extended tofts
    shiftIdx = strcmp(fieldsC,'ETROIPointShift');
end
baseIdx = strcmp(fieldsC,'basepts');
inputC{3}(shiftIdx) = UserInShift;
inputC{3}(baseIdx) = UserInBase;
fid = fopen(filePath,'w+');
fmt = '\r\n%s\t%s\t%d';
for lineNum = 1:size(inputC{1},1)
    col1 = inputC{1}(lineNum);
    col2 = inputC{2}(lineNum);
    col3 = inputC{3}(lineNum);
    fprintf(fid,fmt,col1{1},col2{1},col3);
end
fclose(fid);

%Write statistics
writeStats(outFolder,outfileBase,paramS.model,paramS.cutOff,countV,ktrAll,veAll,kepAll,vpAll,dceRsqAll);

end


filename = 'BulkCaseFine';%'CantileverSquareNewFine';%'LshapeTriFine';%'CantileverSquareNew';'ArchTriFine';%'CantileverSquare';%'ArchTriFine';%'CantileverSquare';%'Lshape';%'LshapeTriFine';%'CantileverSquareSmall';%'';%'Lshape';'CantileverSquare';'Lshape';%'CantileverSquare';%'LshapeFine';%'Bridge_quad_coarse';%'Arch_quad_coarse';%'BridgeCool_Quadrilateral_Bilinear_Structured_Coarse';%'Bridge';%'CantileverSquareSmall';
ptype = 'MACRO';
initial_case = 'given';
m1 = 0.0101;
m2 = 0.0101;
%cost = {'compliance'};
cost = {'stressNorm'};
%cost = {'stressNorm','compliance'};
%weights = [0.55,0.45];
weights = 1;
constraint = {'volumeConstraint'};
filterType = 'P1';
constraint_case = 'EQUALITY';

Vfrac_initial = 0.3;
optimality_initial = 1e-5;
constr_initial = 1e-5;

Vfrac_final = 0.3;
optimality_final = 1e-5;
constr_final = 1e-5;

stressNormExponent_initial = 2;
stressNormExponent_final = 32;

optimizer = 'DualNestedInPrimal';
optimizerUnconstrained = 'PROJECTED GRADIENT';

designVariable = 'MicroParams';
ub = 0.989;
lb = 0.011;
homegenizedVariablesComputer = 'ByVademecum';
%
vademecumFileName = 'SuperEllipseQMax';
%vademecumFileName = 'SuperEllipseQ2';
%vademecumFileName = 'SuperEllipseQOptAnalytic';
%
% designVariable = 'Density';
% homegenizedVariablesComputer = 'ByInterpolation';
% method = 'SIMPALL';
% materialType = 'ISOTROPIC';

kfrac = 2;
nsteps = 100;

plotting = false;
printing = true;
monitoring = true;
monitoring_interval = 1;
maxiter = 1000;


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nick.battista@unc.edu
% Date Created: September 9th, 2016
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting %%	lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us %to add a specific muscle model, please let Nick (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Make_Scallop_Geo_and_Input_Files()

%
% Grid Parameters (MAKE SURE MATCHES IN input2d !!!)
%
Nx =  256;        % # of Eulerian Grid Pts. in x-Direction (MUST BE EVEN!!!)
Lx = 1.0;        % Length of Eulerian Grid in x-Direction
dx = Lx/Nx;      % Grid spatial resolution

%
% Immersed Structure Geometric / Dynamic Parameters %
%
ds = 0.5*dx;              % Lagrangian Pt. Spacing (2x resolution of Eulerian grid)
struct_name = 'scallop'; % Name for .vertex, .spring, etc files. (must match what's in 'input2d')

% Call function to construct geometry
[xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(ds,Lx);

half_len = (length(xLag)-1)/2;   % Computes # of Lag Pts on each "Arm"
ind_off = ceil( 0.4*half_len ); % Computes # of lags from center pt for muscle placement
b_ind = half_len - ind_off;      % Bottom index
t_ind = half_len+2 + ind_off;    % Top index

%b_ind = 1;
%t_ind = length(xLag);
m_dist = sqrt( ( xLag(b_ind)-xLag(t_ind) )^2 + ( yLag(b_ind)-yLag(t_ind) )^2   );

% Test Muscle Placement
plot( xLag( b_ind ), yLag( b_ind ), 'm*'); hold on;
plot( xLag( t_ind ), yLag( t_ind ), 'm*'); hold on;

% Prints .vertex file!
print_Lagrangian_Vertices(xLag,yLag,struct_name);

% Prints .spring file!
k_Spring = 2e6;                    % Spring stiffness (does not need to be equal for all springs)
ds_Rest = ds;                        % Spring resting length (does not need to be equal for all springs)
print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Rest,struct_name);

% Prints .beam file!
k_Beam = 1e11;                      % Beam Stiffness (does not need to be equal for all beams)
C = compute_Curvatures(xLag,yLag); % Computes curvature of initial configuration
print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name);

% Prints .muscle file! [ a_f * Fmax *exp( -( (Q-1)/SK )^2 ) * (1/P0)*(b*P0-a*v)/(v+b); Q = LF/LFO ]
LFO = m_dist; SK = 0.3; a = 0.25; b = 4.0; Fmax = 1e2;
print_Lagrangian_Muscles(xLag,LFO,SK,a,b,Fmax,struct_name,b_ind,t_ind)

% Prints .target file!
%k_Target = 1e7;
%print_Lagrangian_Target_Pts(xLag,k_Target,struct_name);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints VERTEX points to a file called 'struct_name'.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag); % Total # of Lag. Pts

    vertex_fid = fopen([struct_name '.vertex'], 'w');

    fprintf(vertex_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        X_v = xLag(s);
        Y_v = yLag(s);
        fprintf(vertex_fid, '%1.16e %1.16e\n', X_v, Y_v);
    end

    fclose(vertex_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints MUSCLE points to a file called "struct_name".muscle
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Muscles(xLag,LFO,SK,a,b,Fmax,struct_name,ind_b,ind_t)

    %N = length(xLag); %Number of Lagrangian Pts. Total

    muscle_fid = fopen([struct_name '.muscle'], 'w');

    fprintf(muscle_fid, '%d\n', 1 ); % Only 1 muscle

    %MUSCLES BETWEEN VERTICES
    fprintf(muscle_fid, '%d %d %1.16e %1.16e %1.16e %1.16e %1.16e\n', ind_b, ind_t, LFO, SK, a,b,Fmax);

    fclose(muscle_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints SPRING points to a file called 'struct_name'.spring
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Rest,struct_name)

    N = length(xLag);

    spring_fid = fopen([struct_name '.spring'], 'w');

    fprintf(spring_fid, '%d\n', N-1 );    % Print # of springs

    %spring_force = kappa_spring*ds/(ds^2);

    %SPRINGS BETWEEN VERTICES
    for s = 1:N
            if s < N
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds_Rest);
            %else
            %    %Case s=N
            %    fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, 1,   k_Spring, ds_Rest);
            end
    end
    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints TARGET points to a file called 'struct_name'.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name)

    N = length(xLag);

    target_fid = fopen([struct_name '.target'], 'w');

    fprintf(target_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    fclose(target_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints BEAM (Torsional Spring) points to a file called 'struct_name'.beam
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name)

    % k_Beam: beam stiffness
    % C: beam curvature

    N = length(xLag); % NOTE: Total number of beams = Number of Total Lag Pts. - 2

    beam_fid = fopen([struct_name '.beam'], 'w');

    fprintf(beam_fid, '%d\n', N-2 );

    %spring_force = kappa_spring*ds/(ds^2);

    %BEAMS BETWEEN VERTICES
    for s = 2:N-1
            if  s <= N-1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C(s) );
            else
                %Case s=N
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, 1,   k_Beam, C(s) );
            end
    end
    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes "curvature" of starting configuration
%
% NOTE: not curvature in the traditional geometric sense, in the 'discrete'
% sense through cross product.
%
% NOTE: assumes a CLOSED structure
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function C = compute_Curvatures(xLag,yLag)

N = length(xLag);
C = zeros( N );

%Note: needs to be done same order as you print .beam file!
for i=1:N

   % Pts Xp -> Xq -> Xr (same as beam force calc.)

   if ( (i > 1) && (i < N) )

        Xp = xLag(i-1); Xq = xLag(i); Xr = xLag(i+1);
        Yp = yLag(i-1); Yq = yLag(i); Yr = yLag(i+1);

   elseif (i==1)

        Xp = xLag(N); Xq = xLag(i); Xr = xLag(i+1);
        Yp = yLag(N); Yq = yLag(i); Yr = yLag(i+1);

   elseif (i==N)

        Xp = xLag(N-1); Xq = xLag(N); Xr = xLag(1);
        Yp = yLag(N-1); Yq = yLag(N); Yr = yLag(1);

   end

   C(i) = (Xr-Xq)*(Yq-Yp) - (Yr-Yq)*(Xq-Xp); %Cross product btwn vectors

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(ds,Lx)

% ds: Lagrangian pt. spacing
% Lx: Length of Eulerian grid

% The immsersed structure is initially "an angle" of 10 degrees %
ang = 70*pi/180; % original angle
L = 1/10*Lx;     % Structure will be 1/20 of the length of the grid

% Create line of points
x = 0:ds:L;
y = zeros( 1, length( x ) );

% Rotate line of points
[xB,yB] = rotate_geometry_please(x,y,-ang/2);
[xT,yT] = rotate_geometry_please(x,y,ang/2);

% Put together!
xLag = [ xB(end:-1:2) xT ];
yLag = [ yB(end:-1:2) yT ];

[xLag,yLag] = rotate_geometry_please(xLag,yLag,-pi/4);

length(xLag)

xLag = xLag + 0.75*Lx;
yLag = yLag + 0.25*Lx;

% TESTING GEOMETRY
plot(xB,yB,'r*'); hold on;
plot(xT,yT,'go'); hold on;
plot( xLag, yLag, 'k+'); hold on;
axis([0 Lx 0 Lx]);

% for i=1:length(xLag)
%    plot( xLag(i), yLag(i), 'k+'); hold on;
%    axis([0 Lx 0 Lx]);
%    pause();
% end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: rotate set of points
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xN,yN] = rotate_geometry_please(x,y,ang)

% x,y original (x,y) coordinates before rotation
% ang: angle rotating by

xN = zeros( 1, length( x ) ); yN = xN;
for i=1:length(x)
   xN(i) = x(i)*cos(ang) - y(i)*sin(ang);
   yN(i) = x(i)*sin(ang) + y(i)*cos(ang);
end


 function xs = de_pl_osps(x, Gb, ymi, Im, rmi, R, ftab, Gtab, masseff, niter, pixmax, gi, denom)
%function xs = de_pl_osps(x, Gb, ymi, Im, rmi, R, ftab, Gtab, masseff, niter, pixmax, gi, denom)
% The Dual-Energy PL-OS-SPS algorithm for transmission Poisson problem
% (ordered subsets, separable paraboloidal surrogates)
% uses fast precomputed curvatures described in paper fessler::
% in:
%	x		[np,2] initial guess
%	Gb		Gblock object
%	ymi		[nb,na,2] raw polyenergetic measurements
%	Im		[2] source intensities
%	rmi		[nb,na,2] (or scalar) background/scatter
%	R		penalty object (see Robject.m)
%	ftab		F table
%	Gtab		G table
%	masseff		[2,2] effective mass atten coef's
%			2 energies by 2 materials
%	niter		# iterations (including initial guess)
%			fix: allow relaxation parameters here!
%	pixmax		upper constraint for pixel values
%		can be scalar (e.g. 'inf') or an array the size of x
%	gi		sum(Gt)'
%		denom		[np,2] (if precomputed denominator)
% out:
%	x [np,2,niter]	updated image vectors each iteration
%
% Copyright 2001-04-28, Jeff Fessler, The University of Michigan

if nargin < 6, ir_usage, end

if ~isvar('Im') | isempty(Im)
	Im = [1 1];
end
if ~isvar('rmi') | isempty(rmi)
	rmi = zeros(size(ymi));
end

trl_check(ymi(:,:,1), [], rmi(:,:,1));
trl_check(ymi(:,:,2), [], rmi(:,:,2));

if ~isvar('R') | isempty(R), error 'R required', end

nblock = block_ob(Gb, 'n');
if ~isvar('niter')	| isempty(niter),	niter = 1;	end
if ~isvar('pixmax')	| isempty(pixmax),	pixmax = inf;	end
if ~isvar('chat')	| isempty(chat),	chat = 0;	end

starts = subset_start(nblock);
[nb, na, nm] = size(ymi);

%
% precompute denominator
%
if ~isvar('denom'), error 'use de_pl_denom', end

%
% loop over iterations
%
xs = zeros([size(x) niter]);	% [np,2,niter]
x = max(x,0);
xs(:,:,1) = x;
for it=2:niter
	printf('DE-PL-OSPS iteration %d', it)

	%
	% loop over subsets
	%
	for iset=1:nblock
		iblock = starts(iset);
		ia = iblock:nblock:na;

		% s=G*x "mass integrals"
		s1 = Gb{iblock} * x(:,1);
		s2 = Gb{iblock} * x(:,2);

		% predicted meas. means
		s1 = reshape(s1, nb, length(ia));
		s2 = reshape(s2, nb, length(ia));
		fh = ftab.feval(ftab, s1, s2);
		yb1 = Im(1) * exp(-fh(:,:,1)) + rmi(:,ia,1);
		yb2 = Im(2) * exp(-fh(:,:,2)) + rmi(:,ia,2);

		dh1 = ymi(:,ia,1) ./ yb1 - 1;	% m=1
		dh2 = ymi(:,ia,2) ./ yb2 - 1;	% m=2

	if 0
		G11 = interp2(Gtab.s1, Gtab.s2, Gtab.Gm1(:,:,1)', s1, s2);
		G21 = interp2(Gtab.s1, Gtab.s2, Gtab.Gm1(:,:,2)', s1, s2);
		G12 = interp2(Gtab.s1, Gtab.s2, Gtab.Gm2(:,:,1)', s1, s2);
		G22 = interp2(Gtab.s1, Gtab.s2, Gtab.Gm2(:,:,2)', s1, s2);

		% undo nonlinearity associated with table of Gml
		G11 = -exp(-G11);
		G12 = -exp(-G12);
		G21 = -exp(-G21);
		G22 = -exp(-G22);

	else
		% Gml using polynomial model!
		% note: 'coef' is indexed by m, but d_l
if it*iset == 2, warning 'not tested', end
		d11 = reshape(ftab.basis_d1(s1(:), s2(:)) * ftab.coef(:,1), ...
				size(s1));
		d21 = reshape(ftab.basis_d1(s1(:), s2(:)) * ftab.coef(:,2), ...
				size(s1));
		d12 = reshape(ftab.basis_d2(s1(:), s2(:)) * ftab.coef(:,1), ...
				size(s2));
		d22 = reshape(ftab.basis_d2(s1(:), s2(:)) * ftab.coef(:,2), ...
				size(s2));
		G11 = -(yb1-rmi(:,ia,1)) .* d11;
		G21 = -(yb2-rmi(:,ia,2)) .* d21;
		G12 = -(yb1-rmi(:,ia,1)) .* d12;
		G22 = -(yb2-rmi(:,ia,2)) .* d22;
	end

		% g_l is sum over m
		g1 = G11 .* dh1 + G21 .* dh2;
		g2 = G12 .* dh1 + G22 .* dh2;

		grad = Gb{iblock}' * [g1(:) g2(:)];	% [np,2]

%%		for isub=1:1
			for ll=1:2
				pgrad(:,ll) = R{ll}.cgrad(R{ll}, x(:,ll));
				Rdenom(:,ll) = R{ll}.denom(R{ll}, x(:,ll));
			end

			num = nblock * grad - pgrad;
			den = denom + Rdenom;	% semi-constant denom

			x = x + num ./ den;		% the update!
%%			x = x + ((x-xold) .* Lden + num) ./ den; % the update!
			x = max(x,0);		% enforce nonnegativity
			x = min(x,pixmax);	% enforce upper bound constraint
%%		end
	end

	if chat, printf('Range %g %g', min(x), max(x)), end
	xs(:,:,it) = x;
end


function spm_mask(P1, P2, thresh)
% Mask images
% FORMAT spm_mask(P1, P2, thresh)
% P1     - matrix of input image filenames from which
%          to compute the mask.
% P2     - matrix of input image filenames on which
%          to apply the mask.
% thresh - optional threshold(s) for defining the mask.
% The masked images are prepended with the prefix `m'.
%
% If any voxel in the series of images is zero (for data types without
% a floating point representation) or does not have a finite value (for
% floating point and double precision images), then that voxel is set to
% NaN or zero in all the images.  If a threshold, or vector of
% thresholds is passed, then the masking is based on voxels whos
% values are above all the thresholds.
%
% Images sampled in different orientations and positions can be passed
% to the routine.
%__________________________________________________________________________
% Copyright (C) 1999-2011 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_mask.m 4419 2011-08-03 18:42:35Z guillaume $

persistent runonce
if isempty(runonce)
    warning('ImCalc should be preferred to spm_mask whenever possible.');
    runonce = 1;
end

SVNid = '$Rev: 4419 $';

%-Say hello
%--------------------------------------------------------------------------
SPMid = spm('FnBanner',mfilename,SVNid);

%-Parameters & Arguments
%--------------------------------------------------------------------------
if ~nargin
    [P1, sts] = spm_select(Inf,'image','Images to compute mask from');
    if ~sts, return; end
    [P2, sts] = spm_select(Inf,'image','Images to apply mask to');
    if ~sts, return; end
end

if nargin==1
    P2 = P1;
end

V1 = spm_vol(P1);
V2 = spm_vol(P2);

if nargin==3
    if numel(thresh)==1
        thresh = repmat(thresh,numel(V1),1);
    else
        if numel(V1) ~= numel(thresh)
            error('Input argument ''thresh'' has wrong size.');
        end
        thresh = thresh(:);
    end
end

m1 = numel(V1);
m2 = numel(V2);

%-Create headers
%--------------------------------------------------------------------------
VO = V2;
for i=1:m2
    [pth,nm,ext,num] = spm_fileparts(deblank(VO(i).fname));
    VO(i).fname      = fullfile(pth,['m', nm, ext, num]);
    VO(i).descrip    = 'Masked';
    VO(i).mat        = VO(1).mat;
    VO(i).dim(1:3)   = VO(1).dim(1:3);
end
VO  = spm_create_vol(VO);
M   = VO(1).mat;
dim = VO(1).dim(1:3);

%-Compute masked images
%--------------------------------------------------------------------------
spm_progress_bar('Init',VO(1).dim(3),'Masking','planes completed');

for j=1:dim(3)

    msk = true(dim(1:2));
    Mi  = spm_matrix([0 0 j]);

    % Load slice j from all images
    for i=1:m1
        M1  = M\V1(i).mat\Mi;
        %if sum((M1(:)-Mi(:)).^2<eps) M1 = Mi; end;

        img = spm_slice_vol(V1(i),M1,dim(1:2),[0 NaN]);

        msk = msk & isfinite(img);

        if ~spm_type(V1(i).dt(1),'nanrep')
            msk = msk & (img ~= 0);
        end

        if nargin == 3
            msk = msk & (img >= thresh(i));
        end
    end

    % Write the images
    for i=1:m2
        M1        = M\V2(i).mat\Mi;
        img       = spm_slice_vol(V2(i),M1,dim(1:2),[1 0]);
        img(~msk) = NaN;
        VO(i)     = spm_write_plane(VO(i),img,j);
    end

    spm_progress_bar('Set',j);
end

spm_progress_bar('Clear');


% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

% Append Current Temporary Matrix
function MU_uploadTmp(h)
handles = guidata(h.MU_matrix_display);
MatrixName=get(h.Matrix_name_edit,'String');
try
    if ~isempty(h.V.ROI)
        switch h.V.ROI.ROI_flag
            case {7,8,9}
                Flag = MU_load_matrix([MatrixName '_tmp'], h.Mask, 0);
            otherwise
                TMatrix=MU_Crop(handles);
                Flag = MU_load_matrix([MatrixName '_tmp'], TMatrix, 0);
        end
    else
        TMatrix=MU_Crop(handles);
        Flag = MU_load_matrix([MatrixName '_tmp'], TMatrix, 0);
    end
catch me
    error_msg{1,1}='ERROR!!! Saving current matrix aborted.';
    error_msg{2,1}=me.message;
    errordlg(error_msg);
    return;
end

if ~Flag
    errordlg('Uploading temporary matrix failed!');
end

end

function TMatrix=MU_Crop(handles)

if numel(handles.V.DimSize)>=3
    dim = inputdlg(['Please specify up to which dimension to save (2 ~ ' num2str(numel(handles.V.DimSize)) ').'],'Specify Dimension',1,{num2str(numel(handles.V.DimSize))});
    if isempty(dim)
        error('Saving matrix was cancelled.');
    end
    try
        if str2double(dim{1})<2 | str2double(dim{1})>numel(handles.V.DimSize)
            error('The input dimension is invalid.');
        end
        if str2double(dim{1}) == 2
            TMatrix=handles.TMatrix(:,:,handles.V.Slice);
        else
            if str2double(dim{1})+1>numel(handles.V.DimSize)
                dimFlag =[];
            else
                dimFlag=num2str(handles.V.DimPointer(str2double(dim{1})+1:end),',%u');
            end
            eval(['TMatrix=handles.TMatrix(:,:' repmat(',:', [1 str2double(dim{1})-2]) dimFlag ');']);
        end
    catch me
        error('The input dimension is invalid.');
    end
else
    TMatrix=handles.TMatrix(:,:);
end

end


function O = imwarp(I, u, v, nopad)
%IMWARP   Warp image with flow field
%   O = IMWARP(I, U, V[, NOPAD]) warps the image I with optical flow field
%   U and V.  The flow field is to be given in the coordinate system of
%   O, i.e. the operation warps I toward O.  If the optional argument
%   NOPAD is given and true, the undefined pixels (source pixel outside
%   the boundary) are given by the nearest boundary pixels, otherwise
%   they are set to NaN.
%
%   Author:  Stefan Roth, Department of Computer Science, TU Darmstadt
%   Contact: sroth@cs.tu-darmstadt.de
%   $Date: 2007-03-27 14:09:11 -0400 (Tue, 27 Mar 2007) $
%   $Revision: 252 $

% Copyright 2004-2007, Brown University, Providence, RI. USA
% Copyright 2007-2010 TU Darmstadt, Darmstadt, Germany.
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

  % Image size
  sx = size(I, 2);
  sy = size(I, 1);

  % Image size w/ padding
  spx = sx + 2;
  spy = sy + 2;

  if (nargin > 3 & nopad)
    % Warped image coordinates
    [X, Y] = meshgrid(1:sx, 1:sy);
    XI = reshape(X + u, 1, sx * sy);
    YI = reshape(Y + v, 1, sx * sy);

    % Bound coordinates to valid region
    XI = max(1, min(sx - 1E-6, XI));
    YI = max(1, min(sy - 1E-6, YI));

    % Perform linear interpolation (faster than interp2)
    fXI = floor(XI);
    cXI = ceil(XI);
    fYI = floor(YI);
    cYI = ceil(YI);

    alpha_x = XI - fXI;
    alpha_y = YI - fYI;

    O = (1 - alpha_x) .* (1 - alpha_y) .* I(fYI + sy * (fXI - 1)) + ...
        alpha_x .* (1 - alpha_y) .* I(fYI + sy * (cXI - 1)) + ...
        (1 - alpha_x) .* alpha_y .* I(cYI + sy * (fXI - 1)) + ...
        alpha_x .* alpha_y .* I(cYI + sy * (cXI - 1));

  else
    % Pad image with NaNs
    Z = [NaN(1, sx+2); NaN(sy, 1), I, NaN(sy, 1); NaN(1, sx+2)];

    % Warped image coordinates in padded image
    [X, Y] = meshgrid(2:sx+1, 2:sy+1);
    XI = reshape(X + u, 1, sx * sy);
    YI = reshape(Y + v, 1, sx * sy);

    % Bound coordinates to valid region
    XI = max(1, min(spx - 1E-6, XI));
    YI = max(1, min(spy - 1E-6, YI));

    % Perform linear interpolation (faster than interp2)
    fXI = floor(XI);
    cXI = ceil(XI);
    fYI = floor(YI);
    cYI = ceil(YI);

    alpha_x = XI - fXI;
    alpha_y = YI - fYI;

    O = (1 - alpha_x) .* (1 - alpha_y) .* Z(fYI + spy * (fXI - 1)) + ...
        alpha_x .* (1 - alpha_y) .* Z(fYI + spy * (cXI - 1)) + ...
        (1 - alpha_x) .* alpha_y .* Z(cYI + spy * (fXI - 1)) + ...
        alpha_x .* alpha_y .* Z(cYI + spy * (cXI - 1));
  end

  O = reshape(O, sy, sx);


data = [ 10 37 5 6 6];
explode = [ 0 1 0 0 0];
pie(data,explode);
title('\bfExample of a Pie Plot');
legend('One','Two','Three','Four','Five');


function nirfastmesh=readnirfast(filestub)
%
% nirfastmesh=readnirfast(v,f,filestub)
%
% load a group of mesh files saved in the NIRFAST format
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%      filestub: output file stub, output will include multiple files
%          filestub.node: node file
%          filestub.elem: element file to store the surface or tet mesh
%          filestub.param: parameter file
%          filestub.region: node label file
%          filestub.excoef: extinction coeff list
%
% output:
%      nirfastmesh.nodes: node list, 3 columns
%      nirfastmesh.elements: element list, 3 or 4 columns integers
%      nirfastmesh.bndvtx: boundary flag for each node, 1: on the boundary
%      nirfastmesh.region: node segmentation labels
%      nirfastmesh.dimension: dimension of the mesh
%      nirfastmesh.excoef: extinction coeff list
%      nirfastmesh.excoefheader: extinction coeff list field names
%      nirfastmesh.type: the header of the .param file
%      nirfastmesh.prop: optical property list (non-standard, need further processing)
%
%   format definition see http://www.dartmouth.edu/~nir/nirfast/tutorials/NIRFAST-Intro.pdf
%
% example:
%    [node,face,elem]=meshabox([0 0 0],[10 10 10],0.3,1);
%    savenirfast(node,elem,'test', [], ones(size(node)), 'user');
%    mymesh=readnirfast('test')
%    plotmesh([mymesh.nodes mymesh.bndvtx], mymesh.elements,'x>5')
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

fname=[filestub,'.node'];
if(~exist(fullfile(pwd,fname),'file'))
    error([fname ' could not be found']);
end
nirfastmesh.nodes=load(fname);

nirfastmesh.bndvtx=nirfastmesh.nodes(:,1);
nirfastmesh.nodes(:,1)=[];

fname=[filestub,'.elem'];
if(~exist(fullfile(pwd,fname),'file'))
    error([fname ' could not be found']);
end
nirfastmesh.elements=load(fname);
nirfastmesh.dimension=size(nirfastmesh.elements,2)-1;

fname=[filestub,'.region'];
if(exist(fullfile(pwd,fname),'file'))
    nirfastmesh.region=load(fname);
end

fname=[filestub,'.excoef'];
fid=fopen(fname,'rt');
if(fid>=0)
    linenum=0;
    textheader={};
    while(~feof(fid))
        oneline=fgetl(fid);
        linenum=linenum+1;
        [data, count]=sscanf(oneline,'%f');
        if(count>1)
            params=fscanf(fid,repmat('%f ',1,count),inf);
            params=reshape(params,length(params)/count, count);
            params(2:end+1,:)=params;
            params(1,:)=data(:)';
            nirfastmesh.excoef=params;
            nirfastmesh.excoefheader=textheader;
            break;
        else
            textheader{end+1}=oneline;
        end
    end
    fclose(fid);
end

fname=[filestub,'.param'];
fid=fopen(fname,'rt');
if(fid>=0)
    linenum=0;
    params=[];
    while(~feof(fid))
        oneline=fgetl(fid);
        if(linenum==0)
            nirfastmesh.type=oneline;
        end
        linenum=linenum+1;
        [data, count]=sscanf(oneline,'%f');
        if(count>1)
            params=fscanf(fid,repmat('%f ',1,count),inf);
            params=reshape(params,length(params)/count, count);
            params(2:end+1,:)=params;
            params(1,:)=data(:)';
            nirfastmesh.prop=params;
            break;
        end
    end
    fclose(fid);
end


% waveform data parameters
%clear all
close all
%startup
make_figures = true;

% Geographical coordinates
% coords = ...
%     [28.574013,	-80.57236;
%      28.574182,	-80.57241;
%      28.573894,	-80.572352;
%      28.574004,	-80.572561];
coords = ...
    [28.5740171611,	-80.5723755;
     28.5742216111,	-80.5724168556;
     28.5738811889,	-80.572295775;
     28.5740064361,	-80.57256045];

% SLC40 - SpaceX launch complex
% source.lat = 28.562106;
% source.lon = -80.57718;
source.lon = -80.57719; %-80.57718;
source.lat = 28.56195; %28.562106;

easting = [461.091 473.067 447.133 465.217 465.217 465.217];
northing = [1343.9 1306.23 1320.01 1321.26 1321.26 1321.26];
% Wind tower data - could read this from Excel instead
% get Excel file from
relativeHumidity = 89.5; % percent from NASA weather tower data
temperatureF = 80.65; % 80 Fahrenheit according to weather tower data from NASA
wind_direction_from = 144; % degrees - this is the direction FROM according to NASA, see Lisa Huddleston email of Oct 10th
wind_speed_knots = 5.5; % knots

% now call
rocket_airwave_event_analysis


here=pwd;
cd /home/bernard/Data/ABC/animals
attrClasses=dlmread('all-perclass.attributes');
indices=dlmread('split0.mask');
classes=dlmread('all.classid');
cd (here);

trainInstancesIndices=find(indices==1);
trainInstancesLabels=classes(indices==1);
trainClassesIndices=unique(trainInstancesLabels,'stable');

auxLabels=zeros(size(trainInstancesLabels));
for i=1:length(unique(trainInstancesLabels))
    c=find(trainInstancesLabels>0, 1);
    class=trainInstancesLabels(c);
    auxLabels(trainInstancesLabels==class)=i;
    trainInstancesLabels(trainInstancesLabels==class)=0;
end
trainInstancesLabels=auxLabels';

testInstancesIndices=find(indices==0);
testInstancesLabels=classes(indices==0);
testClassesIndices=unique(testInstancesLabels,'stable');

auxLabels=zeros(size(testInstancesLabels));
for i=1:length(unique(testInstancesLabels))
    c=find(testInstancesLabels>0, 1);
    class=testInstancesLabels(c);
    auxLabels(testInstancesLabels==class)=i;
    testInstancesLabels(testInstancesLabels==class)=0;
end
testInstancesLabels=auxLabels';

attrClasses=unique(attrClasses, 'rows', 'stable');
uniqueClasses=unique(classes, 'stable');
newAttrClasses=zeros(size(attrClasses));
for i=1:length(uniqueClasses)
    newAttrClasses(uniqueClasses(i),:)=attrClasses(i,:);
end
attrClasses=newAttrClasses;


function d = size(a,varargin)
% Method 'size' for file_array objects
%__________________________________________________________________________
% Copyright (C) 2005-2017-2012 Wellcome Trust Centre for Neuroimaging

%
% $Id: size.m 7440 2018-10-10 17:28:26Z john $

sa  = struct(a);
nd  = 0;
for i=1:numel(sa)
    nd = max(nd,numel(sa(i).dim));
    nd = max(nd,max(find(sa(i).pos==1)));
end
nd = nd+1;

dim = ones(length(sa),nd);
pos = ones(length(sa),nd);

for i=1:length(sa)
    sz = sa(i).dim;
    dim(i,1:length(sz)) = sz;
    ps = sa(i).pos;
    pos(i,1:length(ps)) = ps;
end

tmp = pos==1;
d   = zeros(1,nd);
for i=1:nd
    ind  = all(tmp(:,[1:(i-1) (i+1):nd]),2);
    d(i) = sum(dim(ind,i));
end
lim = max(max(find(d~=1)),2);
d   = d(1:lim);

if nargin > 1
    d_tmp  = d;
    d      = ones(size(varargin{1}));
    msk    = varargin{1}<=length(d_tmp);
    d(msk) = d_tmp(varargin{1}(msk));
end


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nick.battista@unc.edu
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting %%	lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us %to add a specific muscle model, please let Nick (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the RUBBERBAND-EXAMPLE geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Rubberband()

%
% Grid Parameters (MAKE SURE MATCHES IN input2d !!!)
%
Nx =  32;        % # of Eulerian Grid Pts. in x-Direction (MUST BE EVEN!!!)
Ny =  32;        % # of Eulerian Grid Pts. in y-Direction (MUST BE EVEN!!!)
Lx = 1.0;        % Length of Eulerian Grid in x-Direction
Ly = 1.0;        % Length of Eulerian Grid in y-Direction

% Immersed Structure Geometric / Dynamic Parameters %
N = 2*Nx;        % Number of Lagrangian Pts. (2x resolution of Eulerian grid)
a = 0.2;         % Length of semi-major axis.
b = 0.2;         % Length of semi-minor axis.
ds_Rest = 0;     % Resting length of springs
struct_name = 'rubberband'; % Name for .vertex, .spring, etc files.

% Call function to construct geometry
[xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(N,a,b);

% Plot Geometry to test
plot(xLag,yLag,'r-'); hold on;
plot(xLag,yLag,'*'); hold on;
xlabel('x'); ylabel('y');
axis square;

% Prints .vertex file!
print_Lagrangian_Vertices(xLag,yLag,struct_name);

% Prints .spring file!
%k_Spring = 1e7;
%print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Rest,struct_name);

% Prints .beam file!
%k_Beam = 0.5; C = 0.0;
%print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name);

% Prints .target file!
k_Target = 1e7;
print_Lagrangian_Target_Pts(xLag,k_Target,struct_name);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints VERTEX points to a file called rubberband.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag);

    vertex_fid = fopen([struct_name '.vertex'], 'w');

    fprintf(vertex_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        X_v = xLag(s);
        Y_v = yLag(s);
        fprintf(vertex_fid, '%1.16e %1.16e\n', X_v, Y_v);
    end

    fclose(vertex_fid);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints Vertex points to a file called rubberband.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name)

    N = length(xLag);

    target_fid = fopen([struct_name '.target'], 'w');

    fprintf(target_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    fclose(target_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints BEAM (Torsional Spring) points to a file called rubberband.beam
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name)

    % k_Beam: beam stiffness
    % C: beam curvature

    N = length(xLag); % NOTE: Total number of beams = Number of Total Lag Pts. - 2

    beam_fid = fopen([struct_name '.beam'], 'w');

    fprintf(beam_fid, '%d\n', N );

    %spring_force = kappa_spring*ds/(ds^2);

    %BEAMS BETWEEN VERTICES
    for s = 2:N-1
            if  s <= N-1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C);
            else
                %Case s=N
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, 1,   k_Beam, C);
            end
    end
    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints SPRING points to a file called rubberband.spring
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Rest,struct_name)

    N = length(xLag);

    spring_fid = fopen([struct_name '.spring'], 'w');

    fprintf(spring_fid, '%d\n', N );

    %spring_force = kappa_spring*ds/(ds^2);

    %SPRINGS BETWEEN VERTICES
    for s = 1:N
            if s < N
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds_Rest);
            else
                %Case s=N
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, 1,   k_Spring, ds_Rest);
            end
    end
    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(N,rmin,rmax)

% The immsersed structure is an ellipse %
t = 2*pi/N;
for i=1:N

    xLag(i) = 0.25 + rmax * cos( 2*pi/N*(i-1) );
    yLag(i) = 0.25 + rmin * sin( 2*pi/N*(i-1) );
end


function I = mat2cell_ov(X,xx_s,xx_f,yy_s,yy_f,zz_s,zz_f,overlap,sz)

% converts a matrix into a cell array with overlapping elements
% INPUTS:
% X:            Input matrix
% grid_size:    size of each element without overlap
% overlap:      amount of overlap
% sz:           spatial size of X

% OUTPUT:
% I:            output cell array

% Written by Eftychios A. Pnevmatikakis, Simons Foundation, 2016

I = cell(length(xx_s),length(yy_s),length(zz_s));
nd = length(sz);
if nd == 2; sz(3) = 1; end
for i = 1:length(xx_s)
    for j = 1:length(yy_s)
        for k = 1:length(zz_s)
            extended_grid = [max(xx_s(i)-overlap(1),1),min(xx_f(i)+overlap(1),sz(1)),max(yy_s(j)-overlap(2),1),min(yy_f(j)+overlap(2),sz(2)),max(zz_s(k)-overlap(3),1),min(zz_f(k)+overlap(3),sz(3))];
            if nd == 2
                I{i,j} = X(extended_grid(1):extended_grid(2),extended_grid(3):extended_grid(4),:);
            else
                I{i,j,k} = X(extended_grid(1):extended_grid(2),extended_grid(3):extended_grid(4),extended_grid(5):extended_grid(6),:);
            end
        end
    end
end


% Reg1_test.m
% test of Reg1 regularization object

if 1 % test single-pixel case
	R0 = Reg1(true(1));
	x = 7;
	jf_equal(0, R0.penal(R0, x))
	jf_equal(0, R0.cgrad(R0, x))
	jf_equal(0, R0.denom(R0, x))
	jf_equal(full(R0.C), nan(0,1))
end

if 1 % test tiny image
	R0 = Reg1(true(3,2), 'offsets', '2d:hv');
	x = reshape(1:3*2, 3, 2);
	R0.penal(R0, x);
	R0.cgrad(R0, x);
	R0.denom(R0, x);
	full(R0.C);

	tmp = {true(3,2), 'offsets', '2d:hv'};
	if 0
		rng(7)
		tmp = {tmp{:}, 'user_wt', ones(3,2,2)};
	end
	R0t = Reg1(tmp{:}, 'type_penal', 'mat', 'type_diff', 'ind');
	R0x = Reg1(tmp{:}, 'type_penal', 'mex');
	jf_equal( full(R0t.C), full(R0x.C) )
	jf_equal( full(R0t.C1), full(R0x.C1) )
%	jf_equal( R0t.cgrad(R0t, x), R0x.cgrad(R0x, x) ) % not ?
%	full(R0t.C1)
%	full(R0x.C1)
end

if 1 || ~isvar('Rt'), printm 'Rt'
	if 1 % 3d
		ig = image_geom('nx', 512, 'ny', 480, 'nz', 2^5, 'fov', 500, ...
			'down', 2^2);
	else % 2d
		ig = image_geom('nx', 512, 'ny', 448, 'fov', 500, 'down', 2^3);
	end
	tmp = ig.circ(ig.fov/2*1.1) > 0;
	tmp([1 end],:,:) = 0; tmp(:, [1 end],:) = 0; % zero border for 3d
	ig.mask = tmp; clear tmp
	kappa = 2 + 1 * cos(2*pi*ig.x/ig.fov) * sin(2*pi*ig.y/ig.fov)';
%	kappa = ig.mask;
	f.offsets = ''; f.n_offset = 4;
	if ig.is3
		kappa = repmat(ig.mask_or, [1 1 ig.nz]) .* ig.mask;
		f.offsets = '3d:26';
		f.n_offset = 13;
	else
		kappa = single(kappa) .* ig.mask;
	end

	if 0
		profile on
		cpu etic
		pn = jf_protected_names;
		for qq=1:29*2
			ii = 1:prod(ig.dim);
%			[jx jy jz] = ind2sub(ig.dim, ii);
			jj = pn.ind2sub(ig.dim, ii);
		end
		cpu etoc
		profile report
	return
	end

	f.l2b = 3;
%	f.pot = 'hyper3'; f.delta = 1; f.pot_arg = {f.pot, f.delta};
%	f.pot = 'quad'; f.delta = inf; f.pot_arg = {f.pot};
%	f.pot = 'qgg2'; f.delta = 2; f.pot_arg = {f.pot, f.delta, 1.2};
	f.pot = 'gf1'; f.delta = 2; ...
		tmp = potential_fun('gf1-fit', nan, {'qgg2', [1 10], 1.2});
		f.pot_arg = {f.pot, f.delta, tmp};

%	x = ig.unitv(ig.nx, ig.ny) + ig.unitv(ig.nx/4, 5+0*ig.ny/2);
	x = single(ig.mask + ig.circ(ig.fov/4));
	rng(0)
	x = single(rand(ig.dim)) .* ig.mask;
	if ig.is3
		x = x + ig.unitv ...
			+ ig.unitv(3*ig.nx/8, ig.ny/2, 1) ...
			+ ig.unitv(ig.nx/2, 5*ig.ny/8, ig.nz);
	end
	xm = x(ig.mask);

	for order = 1:2
		pr order
		f.arg1 = {kappa, 'offsets', f.offsets, 'beta', 2^f.l2b, ...
			'edge_type', 'tight', 'order', order};
		if 0 && order == 1 % test user_wt
			rng(7)
%			tmp = ones([ig.dim f.n_offset]);
%			tmp(1:end/4,:,:) = 4;
			tmp = rand([ig.dim f.n_offset]);
			f.arg1 = {f.arg1{:}, 'user_wt', tmp};
		end
		f.arg = {f.arg1{:}, 'pot_arg', f.pot_arg};
		Rt = Reg1(f.arg{:}, 'type_penal', 'mat');
		Rx = Reg1(f.arg{:}, 'type_penal', 'mex'); % default control
%		R1 = Reg1(f.arg{:}, 'type_penal', 'mex', 'control', 1);

		if 0 % test timing of loop control 1 and 2
			% in 3d for order=2 with no user_wt,
			% control=2 is about 5% faster
			g1 = ig.embed(R1.cgrad(R1, xm)); % warm up
			g2 = ig.embed(Rx.cgrad(Rx, xm)); % warm up

			cpu etic
			g1 = ig.embed(R1.cgrad(R1, xm));
			cpu etoc 'Rx cgrad control=1 time'

			cpu etic
			g2 = ig.embed(Rx.cgrad(Rx, xm));
			cpu etoc 'Rx cgrad control=2 time'
			equivs(g1, g2)
		continue
		end

		if has_mex_jf
			switch f.pot
			case {'qgg2', 'gf1'}
				tmp = {f.delta, f.pot_arg{3}};
			otherwise
				tmp = f.delta;
			end
			Ro = Robject(f.arg1{:}, 'type_denom', 'matlab', ...
				'potential', f.pot, 'delta', tmp, ...
				'distance_power', Rt.distance_power);
		end

		if 1 && ig.is3 && has_mex_jf
			zxy = @(x) permute(x, [3 1 2]);
			xyz = @(x) permute(x, [2 3 1]);
			% convert offsets to zxy!
			f.offsets_zxy = reg_offset_xyz_to_zxy(f.offsets, size(kappa));
			Rz = Reg1(zxy(kappa), f.arg{2:end}, ...
				'user_wt', [], ...
				'offsets', f.offsets_zxy, ...
				'offsets_is_zxy', true, ...
				'type_penal', 'zxy');
		end

%		Rm = Rmem(kappa, 'pot_arg', f.pot_arg, 'beta', 2^f.l2b, ...
%			'edge_type', 'tight', ...
%			'distance_power', Rg.distance_power, ...
%			'offsets', f.offsets);

		if 1 % check C1 *
			jf_equal(Rt.C1 * x, Rx.C1 * x)
			jf_equal(Rt.C1 * xm, Rx.C1 * xm)
			jf_equal(ig.shape(Rt.C1 * xm), Rt.C1 * x)
			if has_mex_jf
				jf_equal(Ro.C1 * x, Rx.C1 * x)
			end
		end

		if 0 % check C1 (only for tiny cases)
			c1 = Rt.C1; c1 = c1(:,:);
			c2 = Rx.C1; c2 = c1(:,:);
			jf_equal(c1, c2)
			if has_mex_jf
				co = Ro.C1; co = co(:,:);
				jf_equal(c1, co)
			end
%			clear c1 c2
		end

		if 1 % check dercurv
			[t.dt t.ct] = feval(Rt.dercurv, Rt, Rt.C1 * x);
			[t.dx t.cx] = feval(Rx.dercurv, Rx, Rx.C1 * x);
%			[t.d1 t.c1] = feval(R1.dercurv, R1, R1.C1 * x);
			jf_equal(t.dt, t.dx)
			jf_equal(t.ct, t.cx)
%			jf_equal(t.dt, t.d1)
%			jf_equal(t.ct, t.c1)

			[t.dx t.cx] = feval(Rx.dercurv, Rx, Rx.C1 * xm);
			jf_equal(ig.shape(t.dx), t.dt)
			jf_equal(ig.shape(t.cx), t.ct)

			if has_mex_jf
				[t.do t.co] = feval(Ro.dercurv, Ro, Ro.C1 * xm);
				t.do = ig.shape(t.do);
				t.co = ig.shape(t.co);
				equivs(t.dt, t.do)
				% trick: curv matches *except* at outer edge(s)
				equivs(t.ct(2:end-1,2:end-1,:), t.co(2:end-1,2:end-1,:))
			end
		end

		if 1 && streq(f.pot, 'quad') % check C *
%			tmp = Rt.C * x;
			jf_equal(ig.shape(Rt.C * xm), Rt.C * x)
		end

		if 1 % check penalty value
			tmp = Rt.penal(Rt, x);
			jf_equal(tmp, Rt.penal(Rt, xm))
			equivs(tmp, Rx.penal(Rx, x))
%			equivs(tmp, R1.penal(R1, x))
			jf_equal(Rx.penal(Rx, x), Rx.penal(Rx, xm))
			if has_mex_jf
%				jf_equal(Rt.penal(Rt, xm), Ro.penal(Ro, xm))
				equivs(tmp, Ro.penal(Ro, xm), 'thresh', 18e-6)
			end
		end

		if 1 % check cgrad
			g1 = ig.embed(Rt.cgrad(Rt, xm));
			w1 = ig.embed(Rt.denom(Rt, xm));
			w1s = ig.embed(Rt.denom_sqs1(Rt, xm));
			if 1
				cpu etic
				g2 = Rt.cgrad(Rt, x);
				cpu etoc 'Rt cgrad time'
				jf_equal(g1, g2)
			end

			if has_mex_jf
				go = ig.embed(Ro.cgrad(Ro, xm));
				equivs(g1, go)

				g3 = ig.embed(Rx.cgrad(Rx, xm));
				equivs(g1, g3)

				if 1 % check mex version w/o mask
					cpu etic
					g4 = Rx.cgrad(Rx, x);
					cpu etoc 'Rx cgrad time'
					jf_equal(g3, g4)
				end

				if ig.is3 && order == 1
					tmp = zxy(x);
					% check zxy version w/o mask
					cpu etic
					[g5 w5] = feval(Rz.cgrad_denom, Rz, tmp);
					cpu etoc 'Rz cgrad/denom time'
					g5 = xyz(g5);
					w5 = xyz(w5);
					if isempty(Rx.user_wt) % trick
						equivs(g1, g5)
						equivs(w1, w5)
					end
%					im(w1-w5), return

					% zxy version with mask
					mask_zxy = zxy(ig.mask);
					[g5m w5m] = feval(Rz.cgrad_denom, Rz, tmp(mask_zxy));
					g5m = xyz(embed(g5m, Rz.mask));
					w5m = xyz(embed(w5m, Rz.mask));
					jf_equal(g5, g5m)
					jf_equal(w5, w5m)
				end
			end

			if 0 % check empirical gradient (slow)
				ge = ig.embed(Rx.egrad(Rx, xm, 0.001));
				max_percent_diff(ge, g1)
				im plc 2 3
				im(1, x, 'x'), cbar
				im(2, g1, 'g mat'), cbar
				im(3, g3, 'g mex'), cbar
				im(4, ge, 'g emp'), cbar
				im(5, g1-ge, 'mat-emp'), cbar
				im(6, g3-ge, 'mex-emp'), cbar
			prompt
			end

%			im(g3(:,:,[1 end])-g1(:,:,[1 end]))
%			im clf, im([g1; g3; g1-g3]), cbar

%			clear g1 g2 g3 g4
		end

		if 1 % check denom
			dentf = Rt.denom_sqs1(Rt, x);
			dentm = Rt.denom_sqs1(Rt, xm);
			dentm = ig.embed(dentm);
			equivs(dentf, dentm)

			if has_mex_jf
				denxf = Rx.denom_sqs1(Rx, x);
				denxm = Rx.denom_sqs1(Rx, xm);
				denxm = ig.embed(denxm);
				equivs(dentf, denxf)
				equivs(denxm, denxf)
%				im clf, im([denxf; denxm; denxf-denxm]), cbar

%				denx1 = R1.denom_sqs1(R1, x);
%				equivs(dentf, denx1)

				dentf = Rt.denom(Rt, x);
				denxf = Rx.denom(Rx, x);
%				denx1 = R1.denom(R1, x);
				equivs(dentf, denxf)
%				equivs(dentf, denx1)

				deno = ig.embed(Ro.denom(Ro, xm));
				% trick: denom matches except along outer edges
				ix = (1+order):(ig.nx-order);
				iy = (1+order):(ig.ny-order);
				equivs(dentf(ix,iy), deno(ix,iy))

				if order == 1 && ig.is3 && isempty(Rx.user_wt)
					equivs(dentf, w5) % check zxy
				end
			end
		end

		if 0 % check diag ?
			%d = Rm.diag(R);
		end

		% test threads
	end
prompt
end

return %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Robject designed to match Rmem'
if ~isvar('Ro'), printm 'Ro'
	tmp = repmat(kappa.^2, [1 1 1 length(Rm.offsets)]);
	Ro = Robject(ig.mask, 'edge_type', 'leak', 'beta', 2^f.l2b, ...
		'offsets', Rm.offsets, ...
		'potential', f.pot, 'delta', f.delta, ...
		'type_denom', 'matlab', ...
		'distance_power', 0, 'user_wt', tmp);
	clear tmp
end

cpu etic
g1 = Rm.cgrad(Rm, x);
cpu etoc 'Rm cgrad'
cpu etic
g2 = Ro.cgrad(Ro, x(ig.mask));
g2 = embed(g2, ig.mask);
cpu etoc 'Ro cgrad'
max_percent_diff(g1, g2)
if ~isequal(g1, g2), error 'cgrad mismatch', end, clear g2