diff --git a/src/wt-assign3-2025-5-joint-state-publisher-pkg/src/InverseKinematicNode.cpp b/src/wt-assign3-2025-5-joint-state-publisher-pkg/src/InverseKinematicNode.cpp
index d02f281..db8b298 100644
--- a/src/wt-assign3-2025-5-joint-state-publisher-pkg/src/InverseKinematicNode.cpp
+++ b/src/wt-assign3-2025-5-joint-state-publisher-pkg/src/InverseKinematicNode.cpp
@@ -9,10 +9,12 @@
 class JointPublisherNode: public rclcpp::Node {
 public:
     JointPublisherNode(): Node("ik_node") {
+        L0_ = this->declare_parameter<double>("L0", 0.1105);  // Base height to L2 joint
         L1_ = this->declare_parameter<double>("L1", 0.18);
         L2_ = this->declare_parameter<double>("L2", 0.0435);
         target_x_ = this->declare_parameter<double>("target_x", 0.0);
         target_y_ = this->declare_parameter<double>("target_y", 0.0);
+        target_z_ = this->declare_parameter<double>("target_z", 0.0);
         case_number_ = this->declare_parameter<int>("case_number", 0);
 
         calculateInverseKinematics(case_number_);
@@ -51,6 +53,8 @@ private:
 
     double target_x_;
     double target_y_;
+    double target_z_;
+    double L0_;  // Base height
     double L1_;
     double L2_;
     int case_number_;
@@ -79,6 +83,14 @@ private:
                 target_y_ = param.as_double();
                 recalculate = true;
                 RCLCPP_INFO(this->get_logger(), "Updated target_y to %f", target_y_);
+            } else if (param.get_name() == "target_z") {
+                target_z_ = param.as_double();
+                recalculate = true;
+                RCLCPP_INFO(this->get_logger(), "Updated target_z to %f", target_z_);
+            } else if (param.get_name() == "L0") {
+                L0_ = param.as_double();
+                recalculate = true;
+                RCLCPP_INFO(this->get_logger(), "Updated L0 to %f", L0_);
             } else if (param.get_name() == "L1") {
                 L1_ = param.as_double();
                 recalculate = true;
@@ -114,50 +126,62 @@ private:
     }
 
     void calculateInverseKinematics(int case_number) {
-        double x, y;
+        double x, y, z;
 
-        // Use target_x_ and target_y_ if explicitly set via parameters
+        // Use target_x_, target_y_, target_z_ if explicitly set via parameters
         // Otherwise use predefined case positions
-        bool use_target_params = (target_x_ != 0.0 || target_y_ != 0.0);
+        bool use_target_params = (target_x_ != 0.0 || target_y_ != 0.0 || target_z_ != 0.0);
 
         if (use_target_params) {
             x = target_x_;
             y = target_y_;
+            z = target_z_;
             RCLCPP_INFO(this->get_logger(), "Using explicit target position");
         } else {
             switch(case_number) {
                 case 1:
                     x = 0.20;
                     y = 0.10;
+                    z = 0.05;
                     break;
                 case 2:
                     x = 0.15;
                     y = 0.15;
+                    z = 0.08;
                     break;
                 case 3:
                     x = 0.18;
                     y = 0.05;
+                    z = 0.10;
                     break;
                 case 4:
                     x = 0.12;
                     y = 0.12;
+                    z = 0.06;
                     break;
                 default:
                     x = 0.15;
                     y = 0.10;
+                    z = 0.05;
                     break;
             }
             RCLCPP_INFO(this->get_logger(), "Using case %d position", case_number);
         }
 
-        RCLCPP_INFO(this->get_logger(), "Calculating IK for target position: x=%f, y=%f", x, y);
+        RCLCPP_INFO(this->get_logger(), "Calculating IK for target position: x=%f, y=%f, z=%f", x, y, z);
 
-        // theta_2 (law of cosines)
-        double cos_theta_2 = (x*x + y*y - L1_*L1_ - L2_*L2_) / (2 * L1_ * L2_);
+        // Adjust z for base height (z is measured from world, but we need z from L2 joint)
+        double z_from_base = z - L0_;
+        // Calculate the horizontal reach needed (r) and account for vertical component
+        double r = std::sqrt(x*x + y*y);  // horizontal distance from base
+
+        // theta_2 (law of cosines) - using r and z_from_base for 2D arm planning
+        double reach_distance = std::sqrt(r*r + z_from_base*z_from_base);
+        double cos_theta_2 = (reach_distance*reach_distance - L1_*L1_ - L2_*L2_) / (2 * L1_ * L2_);
 
         // Check if solution is valid
         if (cos_theta_2 < -1.0 || cos_theta_2 > 1.0) {
-            RCLCPP_WARN(this->get_logger(), "target position is out of reach, using default angles");
+            RCLCPP_WARN(this->get_logger(), "target position (r=%f, z=%f) is out of reach, using default angles", r, z_from_base);
             theta_1_ = 0.0;
             theta_2_ = 0.0;
             theta_3_ = 0.0;
@@ -173,19 +197,23 @@ private:
         double k1 = L1_ + L2_ * cos_theta_2;
         double k2 = L2_ * sin_theta_2;
 
+        // Calculate the angle to the target in the r-z plane
+        double alpha = std::atan2(z_from_base, r);
         double gamma = std::atan2(k2, k1);
-        double theta_1 = std::atan2(y, x) - gamma;
+        double theta_1 = alpha - gamma;
+
+        // Base rotation to point towards (x, y)
+        double theta_0 = std::atan2(y, x);
 
         // Convert to robot-specific joint angles
-        double theta_0 = 0.0;  // Base rotation (theta_0)
-        theta_1_ = theta_0;                           // L1 joint
-        theta_2_ = theta_1 - M_PI / 2;                // L2 joint
-        theta_3_ = theta_2;                           // L3 joint
-        theta_4_ = -theta_1 - theta_2 - M_PI / 2;     // L4 joint
-        theta_5_ = theta_0;                           // L5 joint
+        // theta_1_ = theta_0;                           // L1 joint - base rotation
+        theta_2_ = theta_1 - M_PI / 2;                // L2 joint - shoulder
+        theta_3_ = theta_2;                           // L3 joint - elbow
+        theta_4_ = -theta_1 - theta_2 - M_PI / 2;     // L4 joint - wrist to point down
+        theta_5_ = theta_0;                           // L5 joint - wrist rotation
         theta_6_ = 0.0091;                            // L6 joint (fixed)
 
-        RCLCPP_INFO(this->get_logger(), "Calculated angles - theta_1: %f, theta_2: %f", theta_1, theta_2);
+        RCLCPP_INFO(this->get_logger(), "Calculated angles - base: %f, theta_1: %f, theta_2: %f", theta_0, theta_1, theta_2);
     }
 };