using System;

using MathNet.Numerics;

using MathNet.Numerics.LinearAlgebra;

using MathNet.Numerics.LinearRegression;

​

public class Program

{

    public static void Main()

    {

        // simplified example: Expected result: R=1000.0, C=(0, 1000, 0)

        var xValues = new double[]{0.0, 0.0, 1000.0, 0.0};

        var yValues = new double[]{0.0, 1000.0, 1000.0, 1000.0};

        var zValues = new double[]{0.0, 1000.0, 0.0, -1000.0};

        var (radius, x, y, z) = BestFitSphere(xValues, yValues, zValues);

        Console.WriteLine($"radius = {radius} center = ({x},{y},{z})");

        var (standardErrorRadius, r2x, r2y, r2z) = GoodnessOfFitSphere(xValues, yValues, zValues, radius, x, y, z);

        Console.WriteLine($"standardErrorRadius = {standardErrorRadius}\nr2x = {r2x}\nr2y = {r2y}\nr2z = {r2z}");

    }

    public static (double radius, double x, double y, double z) BestFitSphere(double[] xValues, double[] yValues, double[] zValues)

    {

        if ((xValues.Length != yValues.Length) || (yValues.Length != zValues.Length))

            throw new ArgumentException("Array inputs for x y z all need to be the same length");

        var numPoints = xValues.Length;

        var a = new double[numPoints, 4];

        var f = new double[numPoints];

        for(var i = 0; i < numPoints; i++)

        {

            a[i,0] = xValues[i] * 2;

            a[i,1] = yValues[i] * 2;

            a[i,2] = zValues[i] * 2;

            a[i,3] = 1.0;

            f[i] = (xValues[i] * xValues[i]) + (yValues[i] * yValues[i]) + (zValues[i] * zValues[i]);

        }

        var aMatrix = Matrix<double>.Build.DenseOfArray(a);

        var fVector = Vector<double>.Build.DenseOfArray(f);

        var cVector = MultipleRegression.NormalEquations(aMatrix, fVector);

        // solve for the radius

        double t = (cVector[0] * cVector[0]) + (cVector[1] * cVector[1]) + (cVector[2] * cVector[2]) + cVector[3];

        double radius = System.Math.Sqrt(t);

        return (radius, cVector[0], cVector[1], cVector[2]);

    }

    public static (double standardErrorRadius, double r2x, double r2y, double r2z) GoodnessOfFitSphere(double[] xValues, double[] yValues, double[] zValues, double radius, double x, double y, double z)

    {

        if ((xValues.Length != yValues.Length) || (yValues.Length != zValues.Length))

            throw new ArgumentException("Array inputs for x y z all need to be the same length");

        var numPoints = xValues.Length;

        var radiusSquared = radius * radius;

        var radiusModeled = new double[numPoints];

        var radiusObserved = new double[numPoints];

        var xModeled = new double[numPoints];

        var yModeled = new double[numPoints];

        var zModeled = new double[numPoints];

        for(var i = 0; i < numPoints; i++)

        {

            var xDist = xValues[i] - x;

            var yDist = yValues[i] - y;

            var zDist = zValues[i] - z;

            radiusModeled[i] = radius;

            radiusObserved[i] = Math.Sqrt((xDist * xDist) + (yDist * yDist) + (zDist * zDist));

            xModeled[i] = getModeledValue(xValues[i], yValues[i], zValues[i], x, y, z);

            yModeled[i] = getModeledValue(yValues[i], xValues[i], zValues[i], y, x, z);

            zModeled[i] = getModeledValue(zValues[i], yValues[i], xValues[i], z, y, x);

        }

        var standardErrorRadius = GoodnessOfFit.PopulationStandardError(radiusModeled, radiusObserved);

        var r2x = GoodnessOfFit.CoefficientOfDetermination(xModeled, xValues);

        var r2y = GoodnessOfFit.CoefficientOfDetermination(yModeled, yValues);

        var r2z = GoodnessOfFit.CoefficientOfDetermination(zModeled, zValues);

        return (standardErrorRadius, r2x, r2y, r2z);

        double getModeledValue(double measured1, double measured2, double measured3, double center1, double center2, double center3)

        {

            var diff2 = measured2 - center2;

            var diff3 = measured3 - center3;

            var plusOrMinusComponent = Math.Sqrt(radiusSquared - (diff2 * diff2) - (diff3 * diff3));

            var minusSolution = center1 - plusOrMinusComponent;

            var plusSolution = center1 + plusOrMinusComponent;

            var minusDiffAbs = Math.Abs(minusSolution - measured1);

            var plusDiffAbs = Math.Abs(plusSolution - measured1);

            if (minusDiffAbs < plusDiffAbs)

                return minusSolution;

            else

                return plusSolution;

        }

    }

}