function [qout, e] = scara_IK(qini,xg,yg,zg,phig,JA,T04)
%SCARA_IK computes the IF for the scara
%The JA parameter is the 4x4 analytical jacobian with rows x,y,z,rz
%The T04 is the matrix from frame zero to four, i.e. the forward kinematics
%qini is th initial configuration of the procedure
qk = qini
kp=1
K = kp*eye(4)
for k = 1:100
    %compute forward kinematics
    Xe = subs(T04,{q1,q2,d3,q4},{qk});
    x = Xe(1,4);
    y = Xe(2,4);
    z = Xe(3,4);
    phi = atan2(Xe(2,1), Xe(2,2));
    %compute error
    ek(1) = norm([xg-x,yg-y,zg-z]);
    ek(2) = phig-phi;
    qk = qk + JA'*K*ek;
    if e(1) < 0.01  
        qout = qk
        e = ek
        return
    end
end
qout = qk
e = ek

end

