/**
     * Inverse kinematics to get the servo PWMs to reach a certain point, with a given angle
     * of the last segment.
     * @see http://www.aber.ac.uk/~dcswww/Dept/Teaching/CourseNotes/current/CS36510/Inverse_kinematics.pdf
     * @param x X target.
     * @param y Y target.
     * @param z Z target.
     * @param pp4 Global angle of the last segment.
     * @return Result set
     */
    public IKResult getIKForPosition(double x, double y, double z, double pp4) {
        double[] p = new double[4];

        // len1, ... are the lengths of the single segments of the arm
        p[0] = Math.toDegrees(Math.atan(y / x));
        double r = Math.sqrt(x * x + y * y);
        p[3] = Math.toRadians(pp4);
        double rw = r - (len3 * Math.cos(p[3]));
        double z0 = (z - (len3 * Math.sin(p[3]))) - len0;
        double be = Math.atan(z0 / rw);
        double r0 = Math.sqrt(z0 * z0 + rw * rw);
        double al = Math.acos((r0 * r0 + len1 * len1 - len2 * len2) / (2 * r0 * len1));
        p[1] = al + be;
        double ga = Math.acos((len2 * len2 + len1 * len1 - r0 * r0) / (2 * len2 * len1));
        p[2] = Math.toDegrees((p[1] + ga) - Math.PI);

        p[0] = (p[0] > 0) ? (p[0] - 90) : (p[0] + 90);
        p[3] = pp4;
        p[1] = 90 - Math.toDegrees(p[1]);
        p[2] = 90 - p[1] - p[2];
        p[3] = 180.0 - p[1] - p[2] - (90 + p[3]);


        // Angles are transformed to PWM lengths for the servo positions
        // off1, ... are the PWM lengths of a centered position (straight arm)
        // 0.09/0.108 simply is the degrees-to-PWM factor. I shouldnt have
        // put them in here as constant number, but for some reason I did.
        double[] pwms = new double[4];
        if (Math.abs(p[0]) < 90.0) pwms[0] = (p[0] / 0.09 + off1);
        else pwms[0] = ((p[0] / 0.09) - 2500);
        pwms[1] = (off2 - p[1] / 0.09);
        pwms[2] = (p[2] / 0.09 + off3);
        pwms[3] = off4 + p[3] / 0.108;

        int type = IKResult.RESULT_SUCCESS;
        //System.out.println(p[0] + ":" + pwms[0] + ", " + pwms[1] + ", " + pwms[2] + "," + pwms[3]);

        // Check for unreachability
        if (Double.isNaN(pwms[0]) || Double.isNaN(pwms[1]) || Double.isNaN(pwms[2]) || Double.isNaN(pwms[3])) {
            type = IKResult.RESULT_UNREACHABLE;
        } else

        // Check servo ranges
        // Hand servo is excluded from check for better ranges
        if (pwms[0] > rotate.getMaxPWM() || pwms[0] < rotate.getMinPWM()) {
            type = IKResult.RESULT_OUT_OF_RANGE;
            //System.out.println("rotate " + pwms[0]);
        } else
        if (pwms[1] > shoulder.getMaxPWM() || pwms[1] < shoulder.getMinPWM()) {
            type = IKResult.RESULT_OUT_OF_RANGE;
            //System.out.println("shoulder " + pwms[1]);
        } else
        if (pwms[2] > elbow.getMaxPWM() || pwms[2] < elbow.getMinPWM()) {
            type = IKResult.RESULT_OUT_OF_RANGE;
            //System.out.println("elbow " + pwms[2]);
        }
        /*if (pwms[0] > rotate.getMaxPWM() || pwms[0] < rotate.getMinPWM()
                || pwms[1] > shoulder.getMaxPWM() || pwms[1] < shoulder.getMinPWM()
                || pwms[2] > elbow.getMaxPWM() || pwms[2] < elbow.getMinPWM()
                /*|| pwms[3] > hand.getMaxPWM() || pwms[3] < hand.getMinPWM()*///) {


        //    type = IKResult.RESULT_OUT_OF_RANGE;
        //}


        IKResult res = new IKResult(this, p, pwms, type);

        return res;
    }