function q = ikine2(T, q0, DH_para, q1_new, cfg1)

Alpha = DH_para.Alpha;
A = DH_para.A;
D = DH_para.D;
Theta = DH_para.Theta;
Sigma = DH_para.Sigma;

a2 = A(2);
a3 = A(3);

px = T(1, 4); py = T(2, 4); pz = T(3, 4); rz = atan2(T(2, 1), T(2, 2));

a0 = norm([px; py]);
theta0 = atan2(py, px);

q1 = q1_new;
Z2 = [a2 * cos(q1); a2 * sin(q1)];
Z2_P = [px; py] - Z2;
% if cfg1 == 0
%     q2 = acos(dot(Z2, Z2_P) / (norm(Z2) * norm(Z2_P)));
% else
%     q2 = -acos(dot(Z2, Z2_P) / (norm(Z2) * norm(Z2_P)));
% end
q2 = acos(dot(Z2, Z2_P) / (norm(Z2) * norm(Z2_P)));

R_q2 = [cos(q2), -sin(q2); sin(q2), cos(q2)];
Z2_P2 = R_q2 * Z2;
delta_q = acos(dot(Z2_P2, Z2_P) / (norm(Z2_P2) * norm(Z2_P)));
if abs(delta_q) > 0.001
    q2 = -q2;
end
q3 = pz;
q4 = rz - q1 - q2;

q = [q1, q2, q3, q4];

for i = 1:4
    [q0_0(i), count(i)] = wrap_pi(q0(i));
end

for i = 1:4
    if i == 3
        continue
    end
    if q(i) - q0_0(i) > pi
        q(i) = q(i) + (count(i) - 1) * 2 * pi;
    elseif q(i) - q0_0(i) < -pi
        q(i) = q(i) + (count(i) + 1) * 2 * pi;
    else
        q(i) = q(i) + count(i)* 2 * pi;
    end
end