function [act_mvc_lo,act100_mvc_lo] = SangRaas(infile,path1,chans,EMGleg,muscles,plotit,save1,fpcut1,fpcut2)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% A NONLINEAR FILTER FOR ONLINE BAYESIAN ESTIMATION OF
% MYOELECTRIC SIGNALS
% 
% SangRaas.m takes an EMG data structure, preprocesses it, 
% and uses the Sanger and low pass methods and Raasch filter to
% estimate the muscle stimulations and activations of each data stream.  Outputs
% are renormalized to reflect a percentage of maximal effort.
%
% Inputs:
% infile  - input data file to be processed
% path1   - subject initials and date
% chans   - vector of EMG channels to be processed
% EMGleg  - L or R; refers to leg with EMG electrodes
% muscles - structure with EMG channel/muscle correspondence.
% plotit  - 1 if plots are desired, 0 otherwise. 
% save1   - 1 if output is to be saved, 0 otherwise.
% fpcut1   - upper bounds for force plate noise floor (current data)
% fpcut2   - upper bounds for force plate noise floor (fastest trials)
%
% Outputs:
% act_mvc_lo    - output structure with estimated activations, Sanger/MVC/long.
% act100_mvc_lo - output structure with estimated activations, Lowpass100/MVC/long.
%
% Example Usage
% act = SangRaas('walk150_1','AM062411',[1:2 4:9 11:12],'L',muscles,1,1,[25 25],[40 35])
%
% Written by:
% Terence D. Sanger, MD PhD
% Div. Child Neurology and Movement Disorders
% Stanford University Medical Center
% 300 Pasteur, room A345
% Stanford, CA 94305-5235 USA
% (650)736-2154  fax: 725-7459
% sanger@stanford.edu
%
% Edited by Jared Markowitz 
% Small corrections made
% Adapted for gait analysis
% Renormalization added
% Raasch bilinear filter added
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Load data
% addpath '~/Dropbox/harvard/processEMG/preprocess'

fprintf([path1 '/' infile '\n']);

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

if exist('fpcut1') == 1
  input1 = loadEMG(infile,path1,1,chans,EMGleg,fpcut1);
else  
  input1 = loadEMG(infile,path1,1,chans,EMGleg);
end;

% Set parameters
Fs = input1.Fs;
noutputs = Fs;      % output quantization levels
inscale = 4;        % arbitrary input scaling - not used
alpha = 1000/Fs/2;  % sets diffusion rate
beta = 1e-24/(noutputs*Fs)/2;    % sets probability of sudden jumps

% Old parameters:
% alpha = .001/Fs/2;  % sets diffusion rate
% beta = 1e-24/(noutputs*Fs)/2;    % sets probability of sudden jumps

act.subject  = input1.subject;
act.trial    = input1.trial;
act.EMGleg   = input1.EMGleg;
act.Fs       = input1.Fs;
act.FPthresh = input1.FPthresh;
act.Lto      = input1.Lto;
act.Lhs      = input1.Lhs;
act.Rto      = input1.Rto;
act.Rhs      = input1.Rhs;

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

% Preprocess data and compute low pass estimate

input_names = fieldnames(input1);

for jj = 1:length(input_names)  
  name1 = char(input_names(jj));
  if length(name1)>4 && strcmp(name1(1:4),'chan')==1
    fld  = char(input_names(jj));  
    eval(['EMGdat.' fld ' = input1.' fld ';']);
  end;
end;  

fldnames = fieldnames(EMGdat);

eval(['heels = input1.' upper(input1.EMGleg) 'hs;']);
keyboard
for ii = 1:length(fldnames)

  eval(['dat0 = EMGdat.' char(fldnames(ii)) ';']);
  
  % Remove DC offset (mean):
  dat1 = dat0 - mean(dat0);  
  
  % Clip to remove outliers:
  clip = 5*std(dat1);           % Sang: 3, PK: 5.
  ind  = find(abs(dat1)>clip);
  dat2 = dat1;
  dat2(ind) = clip*sign(dat2(ind));

  eval(['mag.' char(fldnames(ii)) '=clip;']);
  
  % Rectify and Normalize:
  dat3 = abs(dat2)/clip;
  
  % Put into structure:
  eval(['preproc.' char(fldnames(ii)) '=dat3;']);

  %%%%%%%%%%%%%%%%%%%%
  % Use the following in normalization:
  
  % Remove 60 Hz noise (notch filter)
  f0    = 59/(Fs/2);
  f1    = 61/(Fs/2);
  [b,a] = butter(2,[f0 f1],'stop');  
  dat4  = filtfilt(b,a,dat1);
    
  % Subtract out moving average to remove motion artifact:
  smooth1 = round(Fs/20);
  if mod(smooth1,2) > 0
    smooth1 = smooth1 - 1;
  end;  
  remdat = smooth(dat4,smooth1);
  dat5  = dat4 - remdat'; 
  
  % Rectify:
  dat6 = abs(dat5);
 
  % Smooth: 
  smooth2 = round(0.1*Fs);
  if mod(smooth2,2) > 0
    smooth2 = smooth2-1;
  end;
  dat7 = smooth(dat6,smooth2);          
  dat8 = max(dat7,0);
  
  eval(['low100.' char(fldnames(ii)) '=dat8;']);
  
  eval(['n_' char(fldnames(ii)) '_lp = [];']);
  
  for qq = 1:(length(heels)-1)
    indstart = heels(qq);
    indstop  = heels(qq+1);
    x1 = 0:1/Fs:(indstop-indstart)/Fs;
    x2 = linspace(0,x1(end),101);
    eval(['n_' char(fldnames(ii)) '_lp=[n_' char(fldnames(ii)) '_lp; interp1(x1,dat8(indstart:indstop),x2,''spline'',''extrap'')];']);     
  end;  
  
  eval(['m_' char(fldnames(ii)) '_lp = mean(n_' char(fldnames(ii)) '_lp,1);']);
  eval(['sd_' char(fldnames(ii)) '_lp = std(n_' char(fldnames(ii)) '_lp,0,1);']);

  eval(['lp_norm1.' char(fldnames(ii)) '=max(m_' char(fldnames(ii)) '_lp);']);  
  eval(['lp_norm2.' char(fldnames(ii)) '=max(m_' char(fldnames(ii)) '_lp+2*sd_' char(fldnames(ii)) '_lp);']);

  eval(['ilow100_1.' char(fldnames(ii)) '= dat8/lp_norm1.' char(fldnames(ii)) ';']);  
  eval(['ilow100_2.' char(fldnames(ii)) '= dat8/lp_norm2.' char(fldnames(ii)) ';']);    
  
  % Preserve sigma comparison just in case
  eval(['sigma_norm.' char(fldnames(ii)) '=std(dat5(heels(1):heels(end)),0);']);

  %%%%%%%%%%%%%%%%%%%%
end;

for iii=1:length(chans)
  eval(['A1{iii}=preproc.' char(fldnames(iii)) ';']);
end;
keyboard
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Run Sanger Bayesian inference algorithm
tic
parfor i = 1:length(chans)
%for i = 1:length(chans)
  i
  % Initialize variables:
  % x is the latent variable (driving rate)
  % MAP is the output estimate
  
  emg = A1{i};
  %eval(['emg = preproc.' char(fldnames(i)) ';']);
  
  x = linspace(1/noutputs, 1, noutputs)'; % Don't start w/ zero bc requires n=0 exactly to match

  MAP = zeros(length(emg),1);        % Store the Bayesian estimates
  g = [alpha -2*alpha alpha];        % Approximate spatial second derivative operator

  % 1. Initialize p(x,0) = 1:
  
  prior = ones(noutputs,1)/noutputs; % Start with uniform prior
    
  for t=1:length(emg) 
    
    % 2. Forward propagate p(x,t-) ~ alpha*(p(x-d,t-1)-2p(x,t-1)+ p(x+d,t-1)) + beta + (1-beta)*p(x,t-1);

    % Compute diffusion term prior1:
    prior1 = zeros(size(prior));
    
    for k = 2:(length(prior)-1)
      prior1(k) = sum(g.*[prior(k-1) prior(k) prior(k+1)]);
    end;
    
    prior1(1)   = alpha*(-2*prior(1) + prior(2));
    prior1(end) = alpha*(prior(end-1) + -2*prior(end));
    
    % Compute jump term prior2:
    prior2 = beta + (1-beta) * prior;      % Sets probability of a sudden jump

    % Add to get total prior:
    prior = prior1+prior2;
    
    % 3. Measure the rectified emg;
   
    emgval = emg(t);        % If this were online, would read a new sample here
    
    % 4. Calculate the posterior likelihood function P(x,t) ~ P(emg|x)p(x,t-):
    
    measurement_model = exp(-emgval./x)./x;   % Exponential model for P(emg|x)
    posterior = measurement_model .* prior;   % Calculate posterior density using Bayes
    
    % 5. Output the signal estimate MAP(x(t)) = argmax P(x,t);
    
    pp = min(find(posterior == max(posterior)));    % Find the maximum of the posterior density
    if (pp > 1 && pp < length(posterior)),          % Interpolate to find the zero 
      dL = posterior(pp-1) - posterior(pp);  
      dR = posterior(pp) - posterior(pp+1); 
      PeakIndex = (pp - .5 - (dL/(dR - dL)));       % Index runs from 1 to noutputs
    else
      PeakIndex = pp;    % If maximum occurs at an endpoint do not interpolate
    end
    MAP(t) = (1/(noutputs-1))*PeakIndex;  % Convert index of peak value to scaled EMG value
    
    % 6. Divide p(x,t) by a constant C so that integral(p(x,t),dx) = 1;
    
    posterior = posterior / sum(posterior);         % Normalize the density
    
    % 7. Repeat from step 2;
    prior = posterior;          % Prior for next iteration is posterior from this iteration
  end;  

  B1{i} = MAP;
  %eval(['sang.' char(fldnames(i)) ' = MAP;']);
    
end
toc
for jjj=1:length(chans)
  eval(['sang.' char(fldnames(jjj)) ' = B1{jjj};']);
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
% No motion artifact, cross talk reduction needed
% If change mind can insert here

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

% Renormalization -  look at notebook!

% First get normalization structure:
norm_struct = define_norm(path1,chans,EMGleg,fpcut2);

for mm = 1:length(chans)
    
  % Scale stimulations to 1:
  name1 = ['chan' num2str(chans(mm))];
  eval([name1 '_s = {};']);
  eval(['n_' name1 '_s = [];']);

  eval(['tempdat1 = sang.' name1 ';']);
  tempdat1 = max(tempdat1,0);
  
  for ll = 1:(length(heels)-1)
    indstart = heels(ll);
    indstop  = heels(ll+1);
    eval([name1 '_s = [' name1 '_s; {tempdat1(indstart:indstop)}];']);
    x1 = 0:1/Fs:(indstop-indstart)/Fs;
    x2 = linspace(0,x1(end),101);
    eval(['n_' name1 '_s = [n_' name1 '_s; interp1(x1,tempdat1(indstart:indstop),x2,''spline'',''extrap'')];']); 
  end;  
    
  eval(['m_' name1 '_s = mean(n_' name1 '_s,1);']);
  eval(['sd_' name1 '_s = std(n_' name1 '_s,0,1);']);
  
  eval(['isang1.' name1 '=tempdat1/max(m_' name1 '_s);']);  
  eval(['isang2.' name1 '=tempdat1/max(m_' name1 '_s+2*sd_' name1 '_s);']);
 
  % Rescale:
  eval(['total_norm_mvc.' name1 '=lp_norm2.' name1 '/max(norm_struct.mvc.' name1 ',norm_struct.fw.' name1 ');']);
  eval(['total_norm_fw.' name1 '=lp_norm2.' name1 '/norm_struct.fw.' name1 ';']);
  eval(['rsang_mvc.' name1 '=isang2.' name1 '*total_norm_mvc.' name1 ';']);
  eval(['rsang_mvc.' name1 '=max(rsang_mvc.' name1 ',0.001);']);
  eval(['rsang_mvc.' name1 '=min(rsang_mvc.' name1 ',0.999);']);
  eval(['rsang_fw.' name1 '=isang2.' name1 '*total_norm_fw.' name1 ';']);
  eval(['rsang_fw.' name1 '=max(rsang_fw.' name1 ',0.001);']);
  eval(['rsang_fw.' name1 '=min(rsang_fw.' name1 ',0.999);']);
  eval(['rlow100_mvc.' name1 '=ilow100_2.' name1 '*total_norm_mvc.' name1 ';']);
  eval(['rlow100_mvc.' name1 '=max(rlow100_mvc.' name1 ',0.001);']);
  eval(['rlow100_mvc.' name1 '=min(rlow100_mvc.' name1 ',0.999);']);
  eval(['rlow100_fw.' name1 '=ilow100_2.' name1 '*total_norm_fw.' name1 ';']);
  eval(['rlow100_fw.' name1 '=max(rlow100_fw.' name1 ',0.001);']);
  eval(['rlow100_fw.' name1 '=min(rlow100_fw.' name1 ',0.999);']);  
end;  

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

% Pass through Raasch bilinear dynamics

chan_names   = fieldnames(sang);
eval(['tt = (0:1/Fs:(length(sang.' chan_names{1} ')-1)/Fs)'';']);
simopts      = simset('FixedStep',1/Fs,'SrcWorkspace','current');
muscle_names = fieldnames(muscles);

for kk = 1:length(muscle_names)
  name1 = muscle_names{kk};  % name1 = 'GAS'
  eval(['chan_no1 = muscles.' name1 ';']);  % chan_no1 = 4
  
  if strcmp(name1,'SOL') == 1
    mfft = 0.20;
  elseif strcmp(name1,'GAS') == 1
    mfft = 0.50;
  elseif strcmp(name1,'TA') == 1
    mfft = 0.35;
  elseif strcmp(name1,'BFSH') == 1    
    mfft = 0.35;
  elseif strcmp(name1,'BFLH') == 1
    mfft = 0.35;
  elseif strcmp(name1,'SMEM') == 1
    mfft = 0.35;
  elseif strcmp(name1,'VAS') == 1
    mfft = 0.50;
  elseif strcmp(name1,'RF') == 1
    mfft = 0.65;
  elseif strcmp(name1,'ILL') == 1    
    mfft = 0.50;
  elseif strcmp(name1,'GLU') == 1
    mfft = 0.45;
  elseif strcmp(name1,'GMED') == 1
    mfft = 0.50;
  elseif strcmp(name1,'GLUU') == 1
    mfft = 0.50;  
  elseif strcmp(name1,'ADDL') == 1
    mfft = 0.35;
  elseif strcmp(name1,'ADDM') == 1
    mfft = 0.45;
  else
    fprintf('Erroneous muscle label.\n');
    return;
  end;
    
  % Short
  k1 = 1/(.01*8.4/(4.8*(1+1.5*mfft)))-1/(.05*8.4/(4.8*(1+1.5*mfft)));
  k2 = 1/(.05*8.4/(4.8*(1+1.5*mfft)));

  eval(['x_in  = [tt rsang_mvc.chan' num2str(chan_no1) '];']);
  sim('raasch.mdl',tt(end),simopts);
  eval(['act_mvc_sh.' name1 ' = a_out;']);

  eval(['x_in  = [tt rsang_fw.chan' num2str(chan_no1) '];']);
  sim('raasch.mdl',tt(end),simopts);
  eval(['act_fw_sh.' name1 ' = a_out;']);

  eval(['x_in  = [tt rlow100_mvc.chan' num2str(chan_no1) '];']);
  sim('raasch.mdl',tt(end),simopts);
  eval(['act100_mvc_sh.' name1 ' = a_out;']);

  eval(['x_in  = [tt rlow100_fw.chan' num2str(chan_no1) '];']);
  sim('raasch.mdl',tt(end),simopts);
  eval(['act100_fw_sh.' name1 ' = a_out;']);

  % Long
  k1 = 1/(.08-.047*mfft)-1/(.09-.056*mfft);
  k2 = 1/(.09-.056*mfft);
  
  eval(['x_in  = [tt rsang_mvc.chan' num2str(chan_no1) '];']);
  sim('raasch.mdl',tt(end),simopts);
  eval(['act_mvc_lo.' name1 ' = a_out;']);

  eval(['x_in  = [tt rsang_fw.chan' num2str(chan_no1) '];']);
  sim('raasch.mdl',tt(end),simopts);
  eval(['act_fw_lo.' name1 ' = a_out;']);

  eval(['x_in  = [tt rlow100_mvc.chan' num2str(chan_no1) '];']);
  sim('raasch.mdl',tt(end),simopts);
  eval(['act100_mvc_lo.' name1 ' = a_out;']);

  eval(['x_in  = [tt rlow100_fw.chan' num2str(chan_no1) '];']);
  sim('raasch.mdl',tt(end),simopts);
  eval(['act100_fw_lo.' name1 ' = a_out;']); 
  
end;

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

% Make plots if desired:

if plotit == 1
  eval(['toes = input1.' upper(input1.EMGleg) 'to;']);
  
  for nn = 1:length(muscle_names)
    name2 = muscle_names{nn};
    eval(['chan_no2 = muscles.' name2 ';']);
    figure
    eval(['plot(tt,preproc.chan' num2str(chan_no2) '*total_norm_mvc.chan' num2str(chan_no2) ');']);
    hold on;
    eval(['plot(tt,rsang_mvc.chan' num2str(chan_no2) ',''g'',''linewidth'',2);']);
    eval(['plot(tt,rlow100_mvc.chan' num2str(chan_no2) ',''c'',''linewidth'',2);']);
    eval(['plot(tt,act_mvc_lo.' name2 ',''r'',''linewidth'',2);']);
    eval(['plot(tt,act100_mvc_lo.' name2 ',''color'',[0.92;0.5;0],''linewidth'',2);']);
    eval(['plot(tt,act_mvc_sh.' name2 ',''color'',[0.5;0;1],''linewidth'',2);']);
    eval(['plot(tt,act100_mvc_sh.' name2 ',''y'',''linewidth'',2);']);    
    for k = 1:length(heels)
      plot([heels(k)/Fs; heels(k)/Fs],[0; 1],'k','linewidth',2);  
    end;  
    for l = 1:length(toes)
      plot([toes(l)/Fs; toes(l)/Fs],[0; 1],'k--','linewidth',2);  
    end;    
    grid on; axis([0 tt(end) 0 1]);
    eval(['title(''' name2 ' Activation Estimate (MVC)'');']);
    xlabel('Time (s)');
    ylabel('Activation Estimate');
    legend('Rectified','Stim (Sang)','Stim (LP)','Act (Sang long)','Act (LP long)','Act (Sang short)','Act (LP short)');
    figure
    eval(['plot(tt,preproc.chan' num2str(chan_no2) '*total_norm_fw.chan' num2str(chan_no2) ');']);
    hold on;
    eval(['plot(tt,rsang_fw.chan' num2str(chan_no2) ',''g'',''linewidth'',2);']);
    eval(['plot(tt,rlow100_fw.chan' num2str(chan_no2) ',''c'',''linewidth'',2);']);
    eval(['plot(tt,act_fw_lo.' name2 ',''r'',''linewidth'',2);']);
    eval(['plot(tt,act100_fw_lo.' name2 ',''color'',[0.92;0.5;0],''linewidth'',2);']);
    eval(['plot(tt,act_fw_sh.' name2 ',''color'',[0.5;0;1],''linewidth'',2);']);
    eval(['plot(tt,act100_fw_sh.' name2 ',''y'',''linewidth'',2);']);    
    for k = 1:length(heels)
      plot([heels(k)/Fs; heels(k)/Fs],[0; 1],'k','linewidth',2);  
    end;  
    for l = 1:length(toes)
      plot([toes(l)/Fs; toes(l)/Fs],[0; 1],'k--','linewidth',2);  
    end;    
    grid on; axis([0 tt(end) 0 1]);
    eval(['title(''' name2 ' Activation Estimate (FW)'');']);
    xlabel('Time (s)');
    ylabel('Activation Estimate');
    legend('Rectified','Stim (Sang)','Stim (LP)','Act (Sang long)','Act (LP long)','Act (Sang short)','Act (LP short)');
    %eval(['save ' name2 '.fig']);
  end;  
end;

% Save if desired:
if save1 == 1
  outdir = ['~/Documents/MATLAB/data/' path1 '/processed/EMG/act_est/combined_new/'];
  if exist(outdir) == 0
    eval(['mkdir ' outdir]);
  end;
  outfile = [outdir '/' input1.trial '.mat'];
  save(outfile,'rsang*','act*','muscles','rlow*');  
end;

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