namespace omp
{
    public class OverlappingPoints : Exception { }

    public static class Calculator
    {
        public const double StationPDOP = -1000;

        public static double[,] Jacobian1T(List<Point> stations, Point position)
        {
            int n = stations.Count;
            double[,] J = new double[4, n];

            for (int i = 0; i < n; ++i)
            {
                double distance = position.Distance(stations[i]);

                if (distance == 0)
                {
                    throw new OverlappingPoints();
                }

                J[0, i] = -(stations[i].x - position.x) / distance;
                J[1, i] = -(stations[i].y - position.y) / distance;
                J[2, i] = -(stations[i].z - position.z) / distance;
                J[3, i] = 1; // Clock bias
            }

            return J;
        }

        public static double PDOP(List<Point> stations, Point position)
        {
            double[,] JR = Jacobian1T(stations, position);

            double[,] mult = MatrixOps.MultTrans(JR);

            double det = MatrixOps.Det(mult);

            double sx2 = MatrixOps.Adj(mult, 0, 0) / det;
            double sy2 = MatrixOps.Adj(mult, 1, 1) / det;
            double sz2 = MatrixOps.Adj(mult, 2, 2) / det;

            return Math.Sqrt(sx2 + sy2 + sz2);
        }

        public static void PDOPs(double[,] pdops, Configurator conf, out double min, out double max)
        {
            min = double.MaxValue;
            max = double.MinValue;

            for (int i = 0; i < pdops.GetLength(0); ++i)
            {
                for (int j = 0; j < pdops.GetLength(1); ++j)
                {
                    try
                    {
                        pdops[i, j] = PDOP(conf.stations, new Point(i, j, conf.zCoordinate));

                        if (pdops[i, j] > max)
                        {
                            max = pdops[i, j];
                        }

                        if (pdops[i, j] < min)
                        {
                            min = pdops[i, j];
                        }
                    }
                    catch (OverlappingPoints)
                    {
                        pdops[i, j] = StationPDOP;
                    }
                }
            }

            Console.WriteLine("{0}...{1}", min, max);
        }
    }
}