function [usat,lambda] = sat(x,uactive,p)
global umax cmax B R S T
g = [ uactive^2 - umax^2 ; uactive*(uactive - cmax*T'*x) ]; % constraints
v = T'*x;       % velocity across semi-active damper
if g(1) <= 0 && g(2) <= 0
    usat = uactive; lambda = [ 0 ; 0 ]; return
elseif uactive*v < 0
    usat = 0; lambda = [ 0 ; -(S'*x+R*usat+B'*p)/(2*usat-cmax*v) ]; return
elseif abs(v) > umax/cmax
    usat = umax*sign(v); lambda = [ -(S'*x+R*usat+B'*p)/(2*usat) ; 0 ]; return
elseif v == 0
    usat = 0; lambda = [ 0 ; 0 ]; return
else
    usat = cmax*v; lambda = [ 0 ; -(R*usat+S'*x+B'*p)/(2*usat-cmax*v) ]; return
end