The FAA has an amazing document, AC 8260.58. I recommend getting the whole PDF, and opening it in a true Adobe reader because this document is interactive, and it is large. This document is full of very well explained math. There are tons of definitions and discussions in here, PLUS there are sample calculators right in the document.Depending on the age of your Adobe reader, you might get some weird document navigation indications.
The document is really about how PBN procedures are designed. PBN procedures assume the aircraft will have an autopilot, so it is interesting to see how the procedures are designed for the limitations of the autopilot.
The drawings in the document are amazing. Look in volume 5 page 9 figures 2-4A and 2-4B, to see how turn errors can be calculated using the radius of the required navigation performance, and bisector lines, and the arc that intersects the edges. The page before (page 4) explains step by step how to make the calculation.
The document is full of acronyms, but they are all explained in volume1 chapter 3. Not all the acronyms are what you might think or are used to in other contexts. In this document CG is "Climb Gradient" not "center of gravity", ATT is "Along Track Tolerance" not that big phone company, etc. It is best to have that chapter book marked to allow you to check back.
The book outlines other tolerances as well. Some of the measurements are metric, while others are nautical miles, and the conversions are part of the calculators in the document. Many of the intermediate values need to be kept in 15 significant digits, and stored in 64bits, with no rounding of intermediate results. There are other common standards that this document relies on, including GPS units calculate the diameter and shape of the earth based on WGS-84 standard.
Much of what is in the document is code that could be copied right into some programs. What code isn't there, is mostly easy to figure out, and could be part of a separate function or method.
There is a good review of many basic algorithms, including intersections of two arcs, locating a point relative to a locus, and calculating arc length, or sub-arc lengths. 8260.58 might be a good workbook for some advanced STEM type program.
With this reference, building an autopilot should be less searching, and easy implementing.