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

using OperationControl.Mapas.Models;

namespace OperationControl.Mapas.Services
{
    public sealed class MapaMovimentoService
    {
        private const double
            RaioTerraMetros =
                6371000.0;


        public MapaMovimentoRover ObterOuCriar(
            MapaEstadoMovimento estado,
            string roverId)
        {
            ValidarEstado(
                estado
            );


            roverId =
                NormalizarId(
                    roverId
                );


            if (
                estado.RoversMutaveis
                    .TryGetValue(
                        roverId,
                        out var existente
                    )
            )
            {
                return existente;
            }


            var movimento =
                new MapaMovimentoRover
                {
                    RoverId =
                        roverId
                };


            estado.RoversMutaveis[
                roverId
            ] =
                movimento;


            return movimento;
        }


        public MapaMovimentoRover? Obter(
            MapaEstadoMovimento estado,
            string roverId)
        {
            ValidarEstado(
                estado
            );


            if (
                string.IsNullOrWhiteSpace(
                    roverId
                )
            )
            {
                return null;
            }


            estado.RoversMutaveis
                .TryGetValue(
                    roverId.Trim(),
                    out var movimento
                );


            return movimento;
        }


        /// <summary>
        /// Atualiza a posição histórica do rover.
        ///
        /// Retorna true somente quando um novo
        /// ponto foi realmente incorporado ao rastro.
        /// </summary>
        public bool AtualizarPosicao(
            MapaEstadoMovimento estado,
            string roverId,
            double latitude,
            double longitude,
            bool pulverizando,
            double distanciaMinimaMetros = 0.05,
            double distanciaMaximaSegmentoMetros = 20.0)
        {
            ValidarCoordenada(
                latitude,
                longitude
            );


            if (
                !double.IsFinite(
                    distanciaMinimaMetros
                ) ||
                distanciaMinimaMetros < 0
            )
            {
                throw new ArgumentOutOfRangeException(
                    nameof(
                        distanciaMinimaMetros
                    )
                );
            }


            if (
                !double.IsFinite(
                    distanciaMaximaSegmentoMetros
                ) ||
                distanciaMaximaSegmentoMetros <= 0
            )
            {
                throw new ArgumentOutOfRangeException(
                    nameof(
                        distanciaMaximaSegmentoMetros
                    )
                );
            }


            var movimento =
                ObterOuCriar(
                    estado,
                    roverId
                );


            var atual =
                new MapaPontoWgs84(
                    latitude,
                    longitude
                );


            var anterior =
                movimento
                    .UltimaPosicaoAceita;


            /*
             * Primeira posição:
             *
             * apenas estabelece a origem.
             * Não existe segmento ainda.
             */
            if (anterior == null)
            {
                movimento.UltimaPosicaoAceita =
                    atual;

                movimento.QuebrarRastro =
                    true;

                return false;
            }


            double distancia =
                DistanciaMetros(
                    anterior,
                    atual
                );


            /*
             * Evita milhares de pontos praticamente
             * iguais por jitter ou alta frequência.
             */
            if (
                distancia <
                distanciaMinimaMetros
            )
            {
                return false;
            }


            /*
             * Salto impossível / reconexão / posição
             * antiga:
             *
             * reposiciona a origem mas NÃO desenha
             * uma linha atravessando o talhão.
             */
            if (
                distancia >
                distanciaMaximaSegmentoMetros
            )
            {
                movimento.UltimaPosicaoAceita =
                    atual;

                movimento.QuebrarRastro =
                    true;

                return false;
            }


            MapaTrechoRastro?
                trechoAtual =
                    movimento
                        .TrechosMutaveis
                        .LastOrDefault();


            bool precisaNovoTrecho =
                movimento.QuebrarRastro ||
                trechoAtual == null ||
                trechoAtual.Pulverizando !=
                    pulverizando;


            if (precisaNovoTrecho)
            {
                trechoAtual =
                    new MapaTrechoRastro(
                        pulverizando
                    );


                /*
                 * Começamos pelo ponto anterior
                 * para manter continuidade visual.
                 */
                trechoAtual
                    .PontosMutaveis
                    .Add(
                        anterior
                    );


                movimento
                    .TrechosMutaveis
                    .Add(
                        trechoAtual
                    );
            }


            trechoAtual
                .PontosMutaveis
                .Add(
                    atual
                );


            movimento.UltimaPosicaoAceita =
                atual;

            movimento.QuebrarRastro =
                false;


            return true;
        }


        /// <summary>
        /// Substitui a previsão MPC atual.
        ///
        /// Menos de dois pontos significa que
        /// não existe previsão desenhável.
        /// </summary>
        public void DefinirPredicao(
            MapaEstadoMovimento estado,
            string roverId,
            IEnumerable<MapaPontoWgs84>?
                pontos)
        {
            var movimento =
                ObterOuCriar(
                    estado,
                    roverId
                );


            var nova =
                new List<MapaPontoWgs84>();


            if (pontos != null)
            {
                foreach (
                    var ponto
                    in pontos)
                {
                    if (ponto == null)
                        continue;


                    ValidarCoordenada(
                        ponto.Latitude,
                        ponto.Longitude
                    );


                    nova.Add(
                        new MapaPontoWgs84(
                            ponto.Latitude,
                            ponto.Longitude
                        )
                    );
                }
            }


            movimento
                .PredicaoMutavel
                .Clear();


            /*
             * Isso também resolve um problema do
             * legado: previsão velha não fica presa
             * quando o MPC para de enviar pontos.
             */
            if (nova.Count >= 2)
            {
                movimento
                    .PredicaoMutavel
                    .AddRange(
                        nova
                    );
            }
        }


        public void LimparRastro(
            MapaEstadoMovimento estado,
            string roverId,
            bool limparPredicao = true)
        {
            var movimento =
                Obter(
                    estado,
                    roverId
                );


            if (movimento == null)
                return;


            movimento
                .TrechosMutaveis
                .Clear();


            /*
             * Mantemos UltimaPosicaoAceita.
             *
             * Assim o próximo ponto parte da posição
             * atual, como o comportamento antigo.
             */
            movimento.QuebrarRastro =
                true;


            if (limparPredicao)
            {
                movimento
                    .PredicaoMutavel
                    .Clear();
            }
        }


        public void Remover(
            MapaEstadoMovimento estado,
            string roverId)
        {
            ValidarEstado(
                estado
            );


            if (
                string.IsNullOrWhiteSpace(
                    roverId
                )
            )
            {
                return;
            }


            estado.RoversMutaveis
                .Remove(
                    roverId.Trim()
                );
        }


        public void Limpar(
            MapaEstadoMovimento estado)
        {
            ValidarEstado(
                estado
            );


            estado.RoversMutaveis
                .Clear();
        }


        private static double DistanciaMetros(
            MapaPontoWgs84 a,
            MapaPontoWgs84 b)
        {
            double lat1 =
                a.Latitude *
                Math.PI / 180.0;

            double lat2 =
                b.Latitude *
                Math.PI / 180.0;

            double dLat =
                (
                    b.Latitude -
                    a.Latitude
                ) *
                Math.PI / 180.0;

            double dLon =
                (
                    b.Longitude -
                    a.Longitude
                ) *
                Math.PI / 180.0;


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


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


            return
                RaioTerraMetros *
                c;
        }


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


        private static string NormalizarId(
            string roverId)
        {
            if (
                string.IsNullOrWhiteSpace(
                    roverId
                )
            )
            {
                throw new ArgumentException(
                    "RoverId inválido.",
                    nameof(roverId)
                );
            }


            return roverId.Trim();
        }


        private static void ValidarEstado(
            MapaEstadoMovimento estado)
        {
            if (estado == null)
            {
                throw new ArgumentNullException(
                    nameof(estado)
                );
            }
        }
    }
}