Inverse kinematics for 4-DOF Articualted manipluator

I am trying to find out the math formulas for Inverse Kinematics for a 4-DOF articulated manipulator.

By Googling I found information about inverse kinematics for 3-DOF, 4-DOF and 6-DOF articulated manipulator, but very few information for 4-DOF robot arm.

But what I found is a paper explaining how to calculate FK and IK for a 4-DOF robot arm:

Kinematics Modeling of a 4-DOF Robotic Arm

I tried to implement the math formula in C code but the calculation doesn't seem to work.

I believe that the formulas are wrong.

The math formula algebraically is the as follow:

dx, dy and dz are the global en effector coordinates.

Angle phi is the end effector orientation.

The C-code looks like this:

#include <stdio.h>
#include <math.h>

#define M_PI   3.14159265358979323846264338327950288

#define deg2Rad(angleInDegrees) ((angleInDegrees) * M_PI / 180.0)

int main()
{

float l1, l2, l3, l4;   // Links mm
float dx, dy, dz;       // EE position
float phi;              // EE orientation
float theta1, theta2, theta3, theta4;   // Joint variables that will be calculated
float A, B, C;

l1 = 170.0;
l2 = 45.0;
l3 = 85.0;
l4 = 130.0;
phi = 0.4;

dx = 30.0;
dy = 15.0;
dz = 20.0;

theta1 = atan(dy/dx);

A = (dx - l4 * cos(theta1) * cos(phi));
B = (dy - l4 * sin(theta1) * cos(phi));
C = (dz - l1 - l4 * sin(phi));

theta3 = acos(((A*A+B*B+C*C)-(l2*l2)-(l3*l3))/(2*l2*l3));

printf("theta1: %f\n", theta1);
printf("A: %f\n", A);
printf("B: %f\n", B);
printf("C: %f\n", C);

printf("theta3: %f\n", theta3);

return 0;
}


When I calculate theta3 I get nand because what is inside acos is greater than 1 and acos can't have a value greater than 1. The conclusion is that something is wrong with the formulas.

Any suggestion?