agrobot_base/AgroBase/OperationControl/Mapas/Services/MapaRotaRetornoService.cs

using OperationControl.Mapas.Models;

namespace OperationControl.Mapas.Services
{
    /// <summary>
    /// Gerencia a rota manual de retorno.
    ///
    /// Não conhece Mapsui.
    /// Não conhece WPF.
    /// Não conhece mouse.
    ///
    /// Internamente trabalha sempre com propriedades
    /// explícitas Latitude/Longitude.
    /// </summary>
    public sealed class MapaRotaRetornoService
    {
        public const double
            ToleranciaRemocaoPadraoMetros =
                2.0;


        /// <summary>
        /// Alterna um ponto:
        ///
        /// - se não existir ponto próximo, adiciona ao final;
        /// - se existir ponto dentro da tolerância, remove o mais próximo.
        ///
        /// Retorna true quando adicionou.
        /// Retorna false quando removeu.
        /// </summary>
        public bool AlternarPonto(
            MapaRotaRetorno rota,
            double latitude,
            double longitude,
            double toleranciaMetros =
                ToleranciaRemocaoPadraoMetros)
        {
            ValidarRota(
                rota
            );

            ValidarCoordenada(
                latitude,
                longitude
            );

            if (
                double.IsNaN(toleranciaMetros) ||
                double.IsInfinity(toleranciaMetros) ||
                toleranciaMetros < 0
            )
            {
                throw new ArgumentOutOfRangeException(
                    nameof(toleranciaMetros),
                    "A tolerância deve ser finita e não negativa."
                );
            }


            var novo =
                new MapaPontoRetorno(
                    latitude,
                    longitude
                );


            int indiceProximo =
                EncontrarIndiceMaisProximo(
                    rota,
                    novo,
                    toleranciaMetros
                );


            if (indiceProximo >= 0)
            {
                rota.PontosMutaveis
                    .RemoveAt(
                        indiceProximo
                    );

                return false;
            }


            rota.PontosMutaveis.Add(
                novo
            );

            return true;
        }


        public void AdicionarPonto(
            MapaRotaRetorno rota,
            double latitude,
            double longitude)
        {
            ValidarRota(
                rota
            );

            ValidarCoordenada(
                latitude,
                longitude
            );


            rota.PontosMutaveis.Add(
                new MapaPontoRetorno(
                    latitude,
                    longitude
                )
            );
        }


        public void Limpar(
            MapaRotaRetorno rota)
        {
            ValidarRota(
                rota
            );

            rota.PontosMutaveis.Clear();
        }


        /// <summary>
        /// Distância acumulada entre os pontos da rota.
        ///
        /// Com zero ou um ponto, retorna 0.
        /// </summary>
        public double ObterDistanciaTotalMetros(
            MapaRotaRetorno rota)
        {
            ValidarRota(
                rota
            );


            if (rota.Quantidade < 2)
                return 0.0;


            double total =
                0.0;


            for (
                int i = 1;
                i < rota.Pontos.Count;
                i++)
            {
                total += DistanciaMetros(
                    rota.Pontos[i - 1],
                    rota.Pontos[i]
                );
            }


            return total;
        }


        /// <summary>
        /// Converte para o contrato legado atual do rover.
        ///
        /// ATENÇÃO:
        /// PontosRetorno trafega como:
        ///
        /// [latitude, longitude]
        ///
        /// Isso é diferente do GeoJSON:
        ///
        /// [longitude, latitude]
        ///
        /// Essa peculiaridade fica confinada SOMENTE aqui.
        /// </summary>
        public List<double[]> ExportarContrato(
            MapaRotaRetorno rota)
        {
            ValidarRota(
                rota
            );


            return rota.Pontos
                .Select(
                    x =>
                        new[]
                        {
                            x.Latitude,
                            x.Longitude
                        }
                )
                .ToList();
        }


        /// <summary>
        /// Reconstrói a rota a partir do contrato recebido
        /// do rover.
        ///
        /// O método é transacional:
        /// somente altera a rota depois que todos
        /// os pontos forem validados.
        /// </summary>
        public void DefinirPeloContrato(
            MapaRotaRetorno rota,
            IEnumerable<double[]>?
                pontosContrato)
        {
            ValidarRota(
                rota
            );


            var novosPontos =
                new List<MapaPontoRetorno>();


            foreach (
                double[] item
                in pontosContrato
                    ?? Enumerable.Empty<double[]>())
            {
                if (
                    item == null ||
                    item.Length < 2
                )
                {
                    throw new InvalidOperationException(
                        "Foi recebido um ponto de retorno incompleto."
                    );
                }


                /*
                 * Contrato atual:
                 *
                 * [0] = latitude
                 * [1] = longitude
                 */
                double latitude =
                    item[0];

                double longitude =
                    item[1];


                ValidarCoordenada(
                    latitude,
                    longitude
                );


                novosPontos.Add(
                    new MapaPontoRetorno(
                        latitude,
                        longitude
                    )
                );
            }


            rota.PontosMutaveis.Clear();

            rota.PontosMutaveis.AddRange(
                novosPontos
            );
        }


        private static int
            EncontrarIndiceMaisProximo(
                MapaRotaRetorno rota,
                MapaPontoRetorno referencia,
                double toleranciaMetros)
        {
            int melhorIndice =
                -1;

            double melhorDistancia =
                double.PositiveInfinity;


            for (
                int i = 0;
                i < rota.Pontos.Count;
                i++)
            {
                double distancia =
                    DistanciaMetros(
                        referencia,
                        rota.Pontos[i]
                    );


                if (
                    distancia <= toleranciaMetros &&
                    distancia < melhorDistancia
                )
                {
                    melhorIndice =
                        i;

                    melhorDistancia =
                        distancia;
                }
            }


            return melhorIndice;
        }


        private static double DistanciaMetros(
            MapaPontoRetorno a,
            MapaPontoRetorno b)
        {
            const double raioTerra =
                6371000.0;


            double lat1 =
                GrausParaRadianos(
                    a.Latitude
                );

            double lat2 =
                GrausParaRadianos(
                    b.Latitude
                );

            double deltaLat =
                GrausParaRadianos(
                    b.Latitude -
                    a.Latitude
                );

            double deltaLon =
                GrausParaRadianos(
                    b.Longitude -
                    a.Longitude
                );


            double h =
                Math.Sin(
                    deltaLat / 2.0
                ) *
                Math.Sin(
                    deltaLat / 2.0
                ) +
                Math.Cos(lat1) *
                Math.Cos(lat2) *
                Math.Sin(
                    deltaLon / 2.0
                ) *
                Math.Sin(
                    deltaLon / 2.0
                );


            double c =
                2.0 *
                Math.Atan2(
                    Math.Sqrt(h),
                    Math.Sqrt(1.0 - h)
                );


            return raioTerra * c;
        }


        private static double
            GrausParaRadianos(
                double graus)
        {
            return graus *
                   Math.PI /
                   180.0;
        }


        private static void
            ValidarCoordenada(
                double latitude,
                double longitude)
        {
            if (
                double.IsNaN(latitude) ||
                double.IsInfinity(latitude) ||
                latitude < -90.0 ||
                latitude > 90.0
            )
            {
                throw new ArgumentOutOfRangeException(
                    nameof(latitude),
                    "Latitude WGS84 inválida."
                );
            }


            if (
                double.IsNaN(longitude) ||
                double.IsInfinity(longitude) ||
                longitude < -180.0 ||
                longitude > 180.0
            )
            {
                throw new ArgumentOutOfRangeException(
                    nameof(longitude),
                    "Longitude WGS84 inválida."
                );
            }
        }


        private static void ValidarRota(
            MapaRotaRetorno rota)
        {
            if (rota == null)
            {
                throw new ArgumentNullException(
                    nameof(rota)
                );
            }
        }
    }
}