Расчет углов. Задание манипулятор

using System;
namespace Manipulation
{
    public static class ManipulatorTask
    {
    static  double UpperArm = 150;
        static  double Forearm = 120;
        static  double Palm = 100;
        static double xc = 0;
        static double yc = 0;
        public static double GetABAngle(double a, double b, double c)//функция находит угол между 2 сторонами в треугольнике 
        {
            if (CheckSides(a, b, c) < 0)
                return double.NaN;
            else
            {
                double angle = (Math.Pow(a, 2) + Math.Pow(b, 2) - Math.Pow(c, 2)) / (2 * a * b);
                return Math.Acos(angle);
            }
        }
        public static int CheckSides(double a, double b, double c) //проверка треугольника на существование 
        {
            if (a <= 0 || b <= 0 || c < 0 || (a > b + c) || (b > c + a) || (c > a + b) ||
                (a == 0 && b == 0 && c == 0))
                return -1;
            return 1;
        }
        public static double[] MoveManipulatorTo(double x, double y, double angle)//основная функция в которой производится нахождение углов
        {
            double[] temp = new double[3];
            var angle_Elbow = Calc_Elbow(x, y, angle);
            var angle_Shoulder = Calc_Shoulder();
            var angle_Wrist = Calc_Wrist(angle, angle_Elbow, angle_Shoulder);
            if (double.IsNaN(angle_Elbow) || double.IsNaN(angle_Shoulder) ||
                double.IsNaN(angle_Wrist))
                temp = new[] { double.NaN, double.NaN, double.NaN };
            temp = new[] {angle_Shoulder, angle_Elbow, angle_Wrist};
            return temp;
        }
        public static double Calc_Elbow(double x, double y, double angle)
        {
            var ac = Calc_ShWr(x, y, angle);
            var angle_Elbow = GetABAngle(UpperArm, Forearm, ac);
            var i = GetPoints(0, 0, UpperArm, xc, yc, Forearm);
            if ((i[0]>= 0 &&i[1] >=0 ) || (i[0] <= 0 && i[1] <= 0))
                return angle_Elbow;
            return -angle_Elbow;
        }
        public static double Calc_ShWr(double x, double y, double angle)//функция считает растояние от точки Shoulder до точки Wrist
        {
            if(x >= 0)
xc = x - Math.Abs(Palm * Math.Cos(angle));
            else
xc = x + Math.Abs(Palm * Math.Cos(angle));
            if(y >= 0)
yc = Math.Abs(y) - Math.Abs(Palm * Math.Sin(angle));
            else
yc = y + Math.Abs(Palm * Math.Sin(angle));
            if ((xc < 1e-12 && xc > 0) || (xc > -1e-12 && xc < 0)) xc = 0;
            if ((yc < 1e-12 && yc > 0) || (yc > -1e-12 && yc < 0)) yc = 0;
            double ac = Math.Sqrt(Math.Pow(xc, 2) + Math.Pow(yc, 2));
            return ac;
        }
        public static double Calc_Shoulder()
        {
            var i = GetPoints(0, 0, UpperArm, xc, yc, Forearm);
            var angle_Shoulder = Math.Acos(i[0] / Math.Sqrt(i[0] * i[0] + i[1] * i[1]));
            if (yc < 0) angle_Shoulder *= -1;
            return angle_Shoulder;
        }
        public static double[] GetPoints(double x1,double y1,double r1,double x2,double y2,double r2)//пересечения 2-х окружностей чтобы найти точку elbow. 
        {
            double x0, y0, d, a, h;
            d = Math.Sqrt(Math.Pow(x1 - x2, 2) + Math.Pow(y1 - y2, 2));
            a = (r1 * r1 - r2 * r2 + d * d) / (2 * d);
            h = Math.Sqrt(Math.Pow(r1, 2) - Math.Pow(a, 2));
            x0 = x1 + a * (x2 - x1) / d;
            y0 = y1 + a * (y2 - y1) / d;
            var x20 = x0 + h * (y2 - y1) / d;
            var y20 = y0 - h * (x2 - x1) / d;
            if (a == r1) return new double[] {x20, y20};
            var x10 = x0 - h * (y2 - y1) / d;
            var y10 = y0 + h * (x2 - x1) / d;
            if(x20 + y20 > x10+y10) return new double[] { x20, y20 };
            else if(x20 + y20 < x10 + y10) return new double[] { x10, y10 };
            else return new double[] { Math.Min(x20,x10), Math.Max(y10, y20) };
        }
        public static double Calc_Wrist(double angle, double angle_Elbow , double angle_Shoulder)//считаю последний угол. Сумма всех углов равна 0.
        {
            var angle_Wrist = -angle - angle_Elbow - angle_Shoulder;
            if (angle_Wrist >= -Math.PI - 1e-11 && angle_Wrist <= Math.PI+ 1e-11)
                return angle_Wrist;
            angle_Wrist = angle_Wrist % Math.PI;
            var d = Math.Abs(Math.Abs(angle_Wrist) - Math.Abs(Math.PI));
            if ((angle_Wrist < 1e-12 && angle_Wrist > 0) ||
                (angle_Wrist > -1e-12 && angle_Wrist < 0)) angle_Wrist = 0;
            else if (angle_Wrist < 0 && d > 1e-9)
                angle_Wrist = angle_Wrist + Math.PI;
            else if (angle_Wrist > 0 && d > 1e-9)
                angle_Wrist = angle_Wrist - Math.PI;
            return angle_Wrist;
        }
    }
}