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)
#define rad2Deg(angleInRadians) ((angleInRadians) * 180.0 / M_PI)

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: %fn", theta1);
   printf("A: %fn", A);
   printf("B: %fn", B);
   printf("C: %fn", C);

   printf("theta3: %fn", 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?