我正在研制一种控制船舵的算法。 我迷失在地理算法中。。。 函数不能正常工作。

direction WhereToMove(double CurrentLatitude, double CurrentLongitude, double TargetLatitude, double TargetLongitude, double azimuth) {
        double azimuthHysteresis = 5; //straight if the deviation is less than 5 degrees
        double pi = 2 * asin(1.0);
        double target = atan2(TargetLatitude - CurrentLatitude, TargetLongitude - CurrentLongitude) * 180 / pi;
        double delta = azimuth - target;
        if (delta > 180) delta -= 360;
        if (delta < -180) delta += 360;
        if (delta < -2) { 
            return right;
        }
        if (delta > 2) {
            return left;
        }
        return straight; // go straight
    }