public static double angleDiff(double target, double current) {
        if (Math.abs(target - current) > Math.PI)
            return 2.0 * Math.PI - (current - target);
        return target - current;
    }