CRPL move code

once
    <-Speed 2.0 div div ( <-Rotation 2.0 div sin ) ->R #diameter of the blind circle
endonce

:MoveToTarget # f f - b
    #moves the unit close to the desired position (pass pixel coordinates) - returns: arrived at destination
    ->ty ->tx

    #compute current angle and distance
    CurrentPixelCoords ->cy ->cx
    <-cx <-cy <-tx <-ty Distance ->distance
    <-ty <-cy sub <-tx <-cx sub atan2 ->desiredAngle
    "D A" <-distance <-desiredAngle Trace3

    #teleport to target if close enought
    <-distance <-Speed lt if
        Self CONST_PIXELCOORDX <-tx SetUnitAttribute
        Self CONST_PIXELCOORDY <-ty SetUnitAttribute
        1 return
    endif

    # do first half of the movement BEFORE the angle is adjusted
    Self CONST_PIXELCOORDX SetUnitAttribute ( <-cx <-angle cos <-Speed 2.0 div mul add )
    Self CONST_PIXELCOORDY SetUnitAttribute ( <-cy <-angle sin <-Speed 2.0 div mul add )

    <-angle <-desiredAngle ShortestAngle ->angleDiff
    <-angleDiff abs <-Rotation lt if
        #move straight
        <-desiredAngle ->angle
    else
        #check for blind spot + save sign
        <-angleDiff 0 gt if
            #turning left
            <-angle PI 2 div add ->blindAngle
            1 ->sign
        else
            #turning right
            <-angle PI 2 div sub ->blindAngle
            -1 ->sign
        endif

        # blind circle position
        <-cx <-blindAngle cos <-R mul add ->bx
        <-cy <-blindAngle sin <-R mul add ->by

        #is the target in the blind spot?
        <-bx <-by <-tx <-ty Distance <-R lt if
            # must reverse the turning direction -> change sign
            "IN CIRCLE" Trace
            <-sign -1 mul ->sign
        endif

        #finally, update angle
        <-angle <-Rotation <-sign mul add ->angle
    endif

    CurrentPixelCoords ->cy ->cx
    # do first half of the movement AFTER the angle is adjusted
    Self CONST_PIXELCOORDX SetUnitAttribute ( <-cx <-angle cos <-Speed 2.0 div mul add )
    Self CONST_PIXELCOORDY SetUnitAttribute ( <-cy <-angle sin <-Speed 2.0 div mul add )