Logic Help - Navigation with gyro + encoders

Hey everybody, I am working on an autonomous tank tread robot based off of RobotShop’s DFrobot Rover v2. I have optical encoders for each track, and a gyro bolted to the middle of the robot. I need to have autonomous navigation, and I know that it is possible to be very accurate with these 3 sensors. What I want to do is have a function that I can give a relative polar coordinate(length in cm, absolute angle) so that I can have the robot turn to that heading, and move that distance. I just want to try and get the logic down right before I jump into any coding, so this is what I am thinking:

 Drive motors have two outputs each: one high/low for direction, and one PWM for speed.
The two encoders are simply Analog In. Code is already in place to count ticks.
The gyroscope is analog in, but gives rate of degree change/second, but I already have running code to keep an absolute angle. Whatever heading the robot is when powered on is 0 degrees.
motorfunction(length in cm, heading in degrees)
set both motors to half speed
if given heading is less than current heading, set left motor to reverse, right motor to foward.
else if set motor left motor to foward, right motor to reverse.
set both motors to 0 speed when given heading == current heading.

set current tick count for both encoders to 0
convert given length in cm into tick count. 
set both motors to full speed, foward.
if current tick count is less than set tick count
    if the current heading is larger than set heading
      set left motor slightly slower until heading is the same.
    if the current heading is less than the set heading
      set right motor slightly slower until heading is the same.

stop when the average between the two encoder tick counts are >= given tick count.

to me, it seems like it would work, but there are already some flaws I can see. the first is that since the gyro output is 0-359 degrees, there can be cases where the robot turns the larger distance, instead of the shorter one. for instance, if the current heading is 5 degrees, and the given heading is 350 degrees, instead of turning 15 degrees counterclockwise, it would turn 345 degrees clockwise. Also, with the turning, there is the issue of over-shooting. How can I stop that from happening too?

should I set both encoders to 0? or should I get the current value for them and set the set value equal to the given value + the current value? are there any benefits to either way?

And then there’s the part that I don’t even know where to begin: what if I have a second navigation mode that were to move while reading other sensors, such as ultrasonic sensors? is there a way that if my robot travels a certain path using ultrasonic sensors to judge wall distances, can I have it turn around and use the encoders/gyro to retrace its steps?

thanks, any help would be greatly appreciated!

Something like this will solve for turning whichever way is shortest.

if destination_heading is less than current_heading
    destination_heading += 360;

If destination_heading - current_heading < 180
    turn clockwise.
    turn counterclockwise.