1function [v_CE, dA, tau_m, F_m, F_CE] = muscle(m, phi, l_CE, A, S)
  2%MUSCLE calculates the muscle dynamics according to [1] and [2].
  3%   INPUT:
  4%   m: muscle parameters (struct)
  5%   phi: joint angles [3 by 1 rad]
  6%   l_CE: length of contractive element [m]
  7%   A: muscle activation []
  8%   S: muscle stimulation []
  9%   OUTPUT:
 10%   v_CE: velocity of contractive element [m/s]
 11%   dA: derivative of muscle activation [/s]
 12%   tau_m: muscle torque [3 by 1 rad]
 13%   F_m: muscle force [N]
 14%   [1] Geyer, H. and H. Herr (2010). "A muscle-reflex model that encodesl_
 15%   principles of legged mechanics produces human walking dynamics and
 16%   muscle activities." Neural Systems and Rehabilitation Engineering, IEEE
 17%   Transactions on 18(3): 263-273.
 18%   [2] Geyer, H., A. Seyfarth, et al. (2003). "Positive force feedback in 
 19%   bouncing gaits?" Proceedings of the Royal Society of London. Series B: 
 20%   Biological Sciences 270(1529): 2173.
 21
 22%#codegen
 23
 24%% model parameters
 25% fixed model parameters
 26N = 1.5; % eccentric force enehancement []
 27K = 5; % cusvature constant []
 28eps_ref = 0.04; % refernece strain of the series elastic element []
 29w = 0.56*m.l_opt; % width [m]
 30c = 0.05; % residual force factor
 31t_ecc = 0.01; % exitation contraction coupling constant [s]
 32
 33% lenght properties MTU
 34% r = [m.r0(1); m.r0(2:3).*cos(phi(2:3)-m.phi_max(2:3))]; % effective lever arm
 35r = [m.r0(1); ...
 36    m.r0(2) .* cos(phi(2) - m.phi_max(2)); ...
 37    m.r0(3) .* cos(phi(3) - m.phi_max(3))];
 38dl_MTU = m.dir .* (...
 39    m.rho .* m.r0(1) .* (phi(1)-m.phi_ref(1)) + ...
 40    m.rho .* m.r0(2) .* (sin(phi(2) - m.phi_max(2)) - sin(m.phi_ref(2) - m.phi_max(2))) + ...
 41    m.rho .* m.r0(3) .* (sin(phi(3) - m.phi_max(3)) - sin(m.phi_ref(3) - m.phi_max(3)))); % deflection of the muscle
 42
 43l_MTU = m.l_opt + m.l_slack + dl_MTU; % total length of the muscle tendon unit
 44l_SE = l_MTU - l_CE; % length of the series elastic element
 45
 46%% activation dynamics
 47dA = (S-A)/t_ecc;
 48
 49%% series elastic element
 50eps_SE = (l_SE-m.l_slack)/m.l_slack;
 51
 52if eps_SE > 0
 53    f_SE = (eps_SE/eps_ref).^2;
 54else
 55    f_SE = 0;
 56end
 57F_SE = f_SE * m.F_max;
 58
 59%% buffer element
 60l_min = m.l_opt - w;
 61eps_BE = w/2;
 62F_BE = m.F_max*(min(0,(l_CE-l_min))/(eps_BE))^2; % buffer element only exites if compressed
 63
 64%% paralel element
 65eps_PE = w;
 66F_PEi = m.F_max*((max(0,l_CE-m.l_opt))/(eps_PE))^2; % paralel element only exites if tensioned
 67
 68%% contractile element
 69% force length relationship
 70f_l = exp(log(c).*abs((l_CE-m.l_opt)./w).^3);
 71
 72% output value based on force length relationship
 73f_v = (F_SE+F_BE)/(A*m.F_max*f_l+F_PEi);
 74
 75% % force length relationship (NOT USED)
 76% if v_CE < 0
 77%     f_v = (m.v_max-v_CE)/(m.v_max+K*v_CE);
 78% else
 79%     f_v = N + (N+1) * (m.v_max + v_CE)/(-m.v_max+7.56*K*v_CE);
 80% end
 81
 82% inverse force velocity relationship
 83if f_v <= 1
 84    v_CE = m.v_max.*(1-f_v)./(K*f_v+1);
 85elseif (f_v > 1) && (f_v <= N)
 86    f_v1 = (f_v-N)/(N-1);
 87    v_CE = m.v_max.*(1+f_v1)./(7.56*K*f_v1-1);
 88else
 89%     v_CE = -m.v_max; 
 90    v_CE = (f_v-N)*0.01-m.v_max;
 91end
 92
 93F_CE = A * f_v * f_l * m.F_max;
 94
 95% muscle force and joint torque
 96F_m = F_SE; % muscle force
 97tau_m = r .* (F_m .* m.dir); % joint torque
 98
 99end
100