  1function [tm, ds, rec_out] = calcreflex(t, N, pm, q, A, l_CE, s_filt, rec, rec_out, c, r, fcglobal)
  2%REFLEXCALCTORQUE calculates the outputs of the reflex model
  3
  4%#codegen
  5
  6lc = length(fcglobal);
  7contacts = [any(fcglobal(1:(lc/2))) any(fcglobal((lc/2+1):end))];
  8refi= [1 2 3; 4 5 6];
  9
 10theta_f = -r.q_ref_f(3);
 11dtheta_f = -r.dq_ref_f(3);
 12
 13if N == 1
 14    qtheta_pto = [-q(3); -q(3)];
 15else
 16    qtheta_pto = rec([29; 30]);
 17end
 18
 19% new implementation of the contact force, where the contact force is
 20% filtered
 21contactForce = [sum(fcglobal((2:3:(lc/2))))/800, ...
 22    sum(fcglobal(lc/2 + (2:3:(lc/2))))/800];
 23ds_Force = [c.filt.A*s_filt(1) + c.filt.B*contactForce(1); ...
 24    c.filt.A*s_filt(2) + c.filt.B*contactForce(2)];
 25contactForceFilt = [c.filt.C*s_filt(1) + c.filt.D*contactForce(1); ...
 26    c.filt.C*s_filt(2) + c.filt.D*contactForce(2)];
 27    
 28%% muscle dynamics
 29% torque vector
 30nMuscles = 7;
 31S = zeros(14, 1);
 32
 33for i = 1:2 % loop left right
 34    % check left foot contact and reflexes
 35    iS = (i-1) * nMuscles;
 36    
 37    if contacts(i)
 38        % stance reflexes
 39        qtheta_pto(i) = -q(3);
 40        
 41        % check double support
 42        not_i = 3-i;
 43        
 44        if contacts(not_i) && (pm(refi(i,1)) < pm(refi(not_i,1)))
 45            DSup = 1;
 46        else
 47            DSup = 0;
 48        end
 49        
 50        Fleg_f = contactForceFilt(i);
 51        Fleg_contra_f = contactForceFilt(not_i);
 52        
 53        % calculate muscle stimulation, limit at 1
 54        S(iS + 1) = c.S_0 ...
 55            + c.m(1).G * r.refForce_s(iS+1); % soleus
 56        % tibialis: here the implementatione is slightly different from
 57        % Geyer. Here the negative feedback is proportional to the average
 58        % soleus and gastrocnemius torque instead of only the soleus
 59        % torque.
 60        S(iS + 2) = c.S_0 ...
 61            + max(0, c.m(2).G * (r.l_CE_s(iS+2)-c.m(2).l_off)) ...
 62            - max(0, c.m(2).G2 * (r.refForce_s(iS+1) + r.refForce_s(iS+3))/2); % tibialis anterior (NEW)
 63%         S(iS + 2) = c.S_0 ...
 64%             + max(0, c.m(2).G * (r.l_CE_s(iS+2)-c.m(2).l_off)) ...
 65%             - max(0, c.m(2).G2 * r.refForce_s(iS+1)); % tibialis anterior (ORIGINAL)
 66        S(iS + 3) = c.S_0 ...
 67            + c.m(3).G * r.refForce_s(iS+3); % gastrocnemius
 68        S(iS + 4) = c.m(4).S_0 ...
 69            + c.m(4).G * r.refForce_m(iS+4) ...
 70            - c.k_phi ...
 71            * (r.qrel_ref_m(refi(i,2)) - c.phi_knee_off)...
 72            * (r.qrel_ref_m(refi(i,2)) > c.phi_knee_off)...
 73            * (r.dqrel_ref_m(refi(i,2)) > 0) ...
 74            - c.k_bw * abs(Fleg_contra_f) * DSup; % vastus
 75        S(iS + 5) = c.S_0_bal +  (...
 76            c.k_p * (theta_f - c.theta_ref) ...
 77            + c.k_d * dtheta_f) ...
 78            * c.k_bw * abs(Fleg_f); % hamstring
 79        S(iS + 6) =  c.S_0_bal + ...
 80            max(0, ...
 81            0.68 * c.k_p * (theta_f - c.theta_ref) ...
 82            + c.k_d * dtheta_f) ...
 83            * c.k_bw * abs(Fleg_f) ...
 84            - c.dS * DSup; % gluteus
 85        S(iS + 7) = (c.S_0_bal ...
 86            - min(0, c.k_p * (theta_f - c.theta_ref) ...
 87            + c.k_d * dtheta_f)) ...
 88            * c.k_bw * abs(Fleg_f) ...
 89            + c.dS * DSup; % hip flexor
 90    else
 91        % swing reflexes
 92        S(iS + 1) = c.S_0; % soleus
 93        S(iS + 2) = c.S_0 ...
 94            + c.m(2).G * (r.l_CE_s(iS+2)-c.m(2).l_off); % tibialis anterior
 95        S(iS + 3) = c.S_0; % gastrocnemius
 96        S(iS + 4) = c.S_0; % vastus
 97        S(iS + 5) = c.S_0 ...
 98            + c.m(5).G * r.refForce_f(iS+5); % hamstring
 99        S(iS + 6) = c.S_0 ...
100            + c.m(6).G * r.refForce_f(iS+6); % gluteus
101        S(iS + 7) = c.S_0 ...
102            + max(0, c.m(7).G * (r.l_CE_f(iS+7)-c.m(7).l_off)) ...
103            - max(0, c.m(7).G2 * (r.l_CE_f(iS+5)-c.m(5).l_off)) ...
104            + c.k_lean * (qtheta_pto(i) - c.theta_ref); % hip flexor
105    end % stance swing reflexes
106end % loop left right
107
108musclenoise = rec_out(31:44) * c.maxnoise;
109S = S + S .* musclenoise;
110
111S = min(1, max(0.01, S));
112if t <= c.dt_f
113    S(:) = 0.01;
114elseif t <= c.dt_m
115    S([1 2 3 4 8 9 10 11]) = 0.01;
116elseif t <= c.dt_s
117    S([1 2 3 8 9 10]) = 0.01;
118end
119
120[v_CE, dA, F_m_out, F_CE, tm] = loopMuscles(c.m, pm(refi), l_CE, A, S);
121tm = -tm;
122
123% extra state derivative output
124ds = [dA; v_CE; ds_Force];
125
126rec_out(1:30) = [F_m_out; F_CE; qtheta_pto];
127rec_out(45:50) = tm;
128
129end
130