How to develop to-point moving algorithm using Gps and compass

I am trying to develop an alghoritm for controlling the rudder of the boat... I got lost in geo algoritms... Function Does not work corectly.

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
    }

A few points:

You could use the constant M_PI for pi

I would imagine you want your angles to be measured clockwise from north. atan2 gives an angle counter clockwise from the x axis. This is simple to fix, use

atan2( dLon, dLat) 

instead of

atan2( dLat, dLon) 

The distance represented by a degree of longitude is, roughly, cos(lat) times the distance represented by a degree of latitude. So you should scale your dlon in the above by cos( M_PI/180.0 * lat). (cos, like all the math functions that deal in angles, takes radians as an argument).

You could simplify computing the difference of azimuth and target by using the math library function remainder, as in

delta = remainder( azimuth-target, 360.0)

this will give a delta between -180 and 180

I don't know if your code will ever be used near 180E. I'd say you should compute the difference in longitudes as if it might, ie use

remainder( TargetLongitude - CurrentLongitude, 360.0)

instead of

TargetLongitude - CurrentLongitude

This might seem OTT, but I've found (the hard way) that its much easier to get into the habit of always computing the difference of longitudes this way than to track down everywhere in the code that such differences are taken when your code is used across 180E.