agrobot_base/AgroBase/AgroBase/Models/KinectModel.cs

using AgroBase.Services;
using System;
using System.Collections.Generic;
using System.Linq;
using static AgroBase.Models.Enums;

namespace AgroBase.Models
{
    public class KinectModel
    {
        public bool Iniciado
        {
            get
            {
                return KinectService.Iniciado;
            }
        }
        public DateTime Momento { get; set; }
        public DateTime UltimoCalculo { get; set; }
        public int AnguloElevacao { get; set; }
        public float AccX { get; set; }
        public float AccY { get; set; }
        public float AccZ { get; set; }
        public float AccW { get; set; }
        public double Pitch { get; set; }
        public double Roll { get; set; }
        public double Yaw { get; set; }
        public double VelX { get; set; }
        public double VelY { get; set; }
        public double VelZ { get; set; }
        public bool Colisao { get; set; }
        public double DistanciaDaEsquerda { get; set; }
        public double DistanciaDaDireita { get; set; }
        public List<Obstaculo> Obstaculos { get; set; }
        public Obstaculo ObstaculoCritico
        {
            get
            {
                if (Obstaculos == null || !Obstaculos.Any())
                {
                    return null;
                }

                // Ordenar os obstáculos por criticidade
                var obstaculoCritico = Obstaculos
                    .OrderByDescending(o => o.DesvioNecessario) // Primeiro, priorizar os que precisam de desvio
                    .ThenBy(o => o.DistanciaMedia_mm) // Priorizar os mais próximos
                    .ThenByDescending(o => o.Largura_mm) // Depois, os mais largos
                    .ThenByDescending(o => o.Altura_mm) // Em seguida, os mais altos
                    .ThenByDescending(o => Math.Abs(o.AnguloParaDesvio)) // Finalmente, os que requerem maior ângulo de desvio
                    .FirstOrDefault();

                return obstaculoCritico;
            }
        }
        public List<Subdivisao> Subdivisoes { get; set; }
        public float EspacoLivreEsquerda
        {
            get
            {
                float d = (KinectService.Parametros.depthFrameWidth / 2);
                float r = KinectService.CalculatePixelToMMRatio(KinectService.Parametros.distProjetarTrajetoriaMm);
                float espacoLivre = d * r;

                var obstaculos = Obstaculos.Where(o => o.DirecaoDesvio == Direcao.Direita).ToList();

                return obstaculos.Any() ? obstaculos.Min(x => x.DistanciaLivreEsquerda) : espacoLivre;
            }
        }
        public float EspacoLivreDireita
        {
            get
            {
                float d = (KinectService.Parametros.depthFrameWidth / 2);
                float r = KinectService.CalculatePixelToMMRatio(KinectService.Parametros.distProjetarTrajetoriaMm);
                float espacoLivre = d * r;

                var obstaculos = Obstaculos.Where(o => o.DirecaoDesvio == Direcao.Esquerda).ToList();

                return obstaculos.Any() ? obstaculos.Min(x => x.DistanciaLivreDireita) : espacoLivre;
            }
        }
        public bool DesvioNecessario
        {
            get
            {
                return Obstaculos.Any(x => x.DesvioNecessario);
            }
        }
        public double AnguloDesvio { get; set; }
        public Direcao DirecaoDesvio
        {
            get
            {
                var ObstaculosDetectados = Obstaculos.Where(x => x.DesvioNecessario).ToList();

                if (!ObstaculosDetectados.Any())
                {
                    AnguloDesvio = 0;
                    return Direcao.Frente;
                }
                if (ObstaculosDetectados.Any(x => x.DirecaoDesvio == Direcao.Parado))
                {
                    AnguloDesvio = 0;
                    return Direcao.Parado;
                }
                if (ObstaculosDetectados.Count() == 1)
                {
                    AnguloDesvio = ObstaculoCritico.AnguloParaDesvio;
                    return ObstaculoCritico.DirecaoDesvio;
                }

                var obstaculosLista = ObstaculosDetectados.Select(o => o.Clone()).ToList();
                float LimiteRiscoAceitavel = 0.1f;

                //var Simulacoes = RealizarSimulacao(obstaculosLista, LimiteRiscoAceitavel);
                var Simulacoes = RealizarSimulacaoGranular(obstaculosLista, 10, LimiteRiscoAceitavel);

                if (Simulacoes.Any(x => x.DirecaoDesvio == Direcao.Parado || x.RiscoTotal > LimiteRiscoAceitavel))
                {
                    AnguloDesvio = 0;
                    return Direcao.Parado;
                }

                // Se a primeira simulação for segura
                var SimulacaoSegura = Simulacoes.First();
                AnguloDesvio = SimulacaoSegura.AnguloDesvio;
                return SimulacaoSegura.DirecaoDesvio;

                //AnguloDesvio = ObstaculoCritico.AnguloParaDesvio;
                //return ObstaculoCritico.DirecaoDesvio;
            }
        }
        public bool StatusDetecao
        {
            get
            {
                return DesvioNecessario && DirecaoDesvio != Direcao.Frente;
            }
        }
        public bool UltimoStatusDetecao { get; set; }
        public DateTime UltimoComandoRespondido { get; set; }
        public void AtualizarUltimoStatusDeteccao()
        {
            if (StatusDetecao != UltimoStatusDetecao)
            {
                UltimoStatusDetecao = StatusDetecao;
            }
        }

        private List<SimulacaoDesvio> RealizarSimulacao(List<Obstaculo> obstaculosSimulados, float LimiteRiscoAceitavel)
        {
            List<SimulacaoDesvio> simulacoes = new List<SimulacaoDesvio>();

            while (obstaculosSimulados.Any())
            {
                var obstaculoCritico = obstaculosSimulados.OrderBy(x => x.DistanciaMedia_mm).First();

                SimulacaoDesvio simulacaoAtual = new SimulacaoDesvio
                {
                    Obstaculos = obstaculosSimulados.Select(o => o.Clone()).ToList()
                };

                if (!simulacaoAtual.Obstaculos.Any())
                {
                    break;
                }

                // Realiza a simulação para a próxima iteração
                simulacaoAtual = SimularDesvio(simulacaoAtual.Obstaculos);
                simulacoes.Add(simulacaoAtual);

                // Se a simulação atual tiver risco aceitável, continuamos a processar
                if (simulacaoAtual.RiscoTotal >= LimiteRiscoAceitavel)
                {
                    break; // Se o risco for alto, parar a simulação
                }

                // Atualiza os obstáculos para a próxima simulação
                obstaculosSimulados = simulacaoAtual.Obstaculos;
            }

            return simulacoes;
        }

        private SimulacaoDesvio SimularDesvio(List<Obstaculo> obstaculosSimulados)
        {
            var obstaculosClone = obstaculosSimulados.Select(o => o.Clone()).ToList();
            var obstaculoDesvio = obstaculosClone.OrderBy(o => o.DistanciaMedia_mm).First();
            obstaculosClone.Remove(obstaculoDesvio);

            double novoAnguloDesvio = obstaculoDesvio.AnguloParaDesvio;

            foreach (var obstaculo in obstaculosClone)
            {
                int deslocamentoPx;
                float novaDistancia = obstaculo.DistanciaMedia_mm - obstaculoDesvio.DistanciaMedia_mm;
                float ratioAtual = KinectService.CalculatePixelToMMRatio(obstaculo.DistanciaMedia_mm);
                float ratioNovo = KinectService.CalculatePixelToMMRatio(novaDistancia);
                float fatorCrescimento = ratioAtual / ratioNovo;

                // Atualizar largura e altura
                int novaLarguraPx = Convert.ToInt32(obstaculo.Width * fatorCrescimento);
                int novaAlturaPx = Convert.ToInt32(obstaculo.Height * fatorCrescimento);

                // Calcular o deslocamento em X e Y para a nova posição
                int deslocamentoX = (obstaculo.Width - novaLarguraPx) / 2;
                int deslocamentoY = (obstaculo.Height - novaAlturaPx) / 2;

                // Atualizar as coordenadas X e Y com o deslocamento
                int novoX = obstaculo.X + deslocamentoX;
                int novoY = obstaculo.Y - deslocamentoY;

                obstaculo.X = novoX;
                obstaculo.Y = novoY;
                obstaculo.Width = novaLarguraPx;
                obstaculo.Height = novaAlturaPx;
                obstaculo.DistanciaMedia_mm = novaDistancia;

                novoAnguloDesvio += obstaculo.AnguloParaDesvio;
            }

            double riscoTotal = CalcularRiscoTotal(obstaculosClone);

            SimulacaoDesvio simulacao = new SimulacaoDesvio
            {
                DirecaoDesvio = obstaculoDesvio.DirecaoDesvio,
                AnguloDesvio = novoAnguloDesvio,
                RiscoTotal = riscoTotal,
                Obstaculos = obstaculosClone
            };

            return simulacao;
        }

        private double CalcularRiscoTotal(List<Obstaculo> obstaculos)
        {
            double riscoTotal = 0;

            foreach (var obstaculo in obstaculos)
            {
                if (obstaculo.DirecaoDesvio != Direcao.Parado)
                {
                    riscoTotal += (1 / obstaculo.DistanciaMedia_mm) * 1; // obstaculo.PercentualCobertura;
                }
                else
                {
                    riscoTotal += 1;
                }
            }

            return riscoTotal;
        }



        /*private List<SimulacaoDesvio> RealizarSimulacaoGranular(List<Obstaculo> obstaculosSimulados, float passoSimulacaoCm, float LimiteRiscoAceitavel)
        {
            List<SimulacaoDesvio> simulacoes = new List<SimulacaoDesvio>();
            var obstaculosClone = obstaculosSimulados.Select(o => o.Clone()).ToList();
            float distanciaPercorrida = 0;

            while (obstaculosClone.Any())
            {
                var obstaculoCritico = obstaculosClone.OrderBy(o => o.DistanciaMedia_mm).First();

                // Simular um passo
                SimulacaoDesvio simulacaoAtual = SimularPasso(obstaculoCritico.DirecaoDesvio, obstaculoCritico.AnguloParaDesvio, obstaculosClone, passoSimulacaoCm);
                simulacoes.Add(simulacaoAtual);

                distanciaPercorrida += passoSimulacaoCm * 10; // Convertendo de cm para mm

                // Remover obstáculos que já foram ultrapassados
                obstaculosClone = simulacaoAtual.Obstaculos.Where(o => o.DistanciaMedia_mm > distanciaPercorrida).ToList();

                if (simulacaoAtual.RiscoTotal >= LimiteRiscoAceitavel || !obstaculosClone.Any())
                {
                    break;
                }
            }

            return simulacoes;
        }

        private SimulacaoDesvio SimularPasso(Direcao direcaoDesvio, double anguloDesvio, List<Obstaculo> obstaculosSimulados, float passoSimulacaoCm)
        {
            var obstaculosClone = obstaculosSimulados.Select(o => o.Clone()).ToList();
            double novoAnguloDesvio = anguloDesvio;

            foreach (var obstaculo in obstaculosClone)
            {
                // Calcular o deslocamento granular em X e Y com base na direção e no passo da simulação
                int deslocamentoPx = Convert.ToInt32(KinectService.CalculatePixelToMMRatio(obstaculo.DistanciaMedia_mm) / passoSimulacaoCm);

                if (direcaoDesvio == Direcao.Esquerda)
                {
                    obstaculo.X -= deslocamentoPx;
                }
                else if (direcaoDesvio == Direcao.Direita)
                {
                    obstaculo.X += deslocamentoPx;
                }

                // Atualizar a distância considerando o passo da simulação
                obstaculo.DistanciaMedia_mm -= passoSimulacaoCm * 10; // Convertendo de cm para mm

                // Atualizar as novas dimensões do obstáculo com base na nova distância
                float ratioAtual = KinectService.CalculatePixelToMMRatio(obstaculo.DistanciaMedia_mm);
                float fatorCrescimento = KinectService.CalculatePixelToMMRatio(obstaculo.DistanciaMedia_mm + (passoSimulacaoCm * 10)) / ratioAtual;
                obstaculo.Width = Convert.ToInt32(obstaculo.Width * fatorCrescimento);
                obstaculo.Height = Convert.ToInt32(obstaculo.Height * fatorCrescimento);

                //novoAnguloDesvio += obstaculo.AnguloParaDesvio;
            }

            double riscoTotal = CalcularRiscoTotal(obstaculosClone);

            return new SimulacaoDesvio
            {
                DirecaoDesvio = direcaoDesvio,
                AnguloDesvio = novoAnguloDesvio,
                RiscoTotal = riscoTotal,
                Obstaculos = obstaculosClone
            };
        }*/

        private List<SimulacaoDesvio> RealizarSimulacaoGranular(List<Obstaculo> obstaculosSimulados, float passoSimulacaoCm, float LimiteRiscoAceitavel)
        {
            List<SimulacaoDesvio> simulacoes = new List<SimulacaoDesvio>();
            var obstaculosClone = obstaculosSimulados.Select(o => o.Clone()).ToList();
            float distanciaPercorrida = 0;
            int passo = 0;

            while (obstaculosClone.Any())
            {
                var obstaculoCritico = obstaculosClone.OrderBy(o => o.DistanciaMedia_mm).First();

                // Simular um passo
                SimulacaoDesvio simulacaoAtual = SimularPasso(obstaculoCritico.DirecaoDesvio, obstaculoCritico.AnguloParaDesvio, obstaculosClone, passoSimulacaoCm, ref passo);
                simulacoes.Add(simulacaoAtual);

                distanciaPercorrida += passoSimulacaoCm * 10; // Convertendo de cm para mm

                // Verificar se o robô ultrapassou o obstáculo crítico
                if (distanciaPercorrida >= (obstaculoCritico.DistanciaMedia_mm - KinectService.Parametros.minDepth))
                {
                    // Novo obstáculo crítico, ajustar o ângulo de desvio
                    obstaculosClone = simulacaoAtual.Obstaculos.Where(o => o.DistanciaMedia_mm > distanciaPercorrida).ToList();

                    if (obstaculosClone.Any())
                    {
                        obstaculoCritico = obstaculosClone.OrderBy(o => o.DistanciaMedia_mm).First();
                        simulacaoAtual = SimularPasso(obstaculoCritico.DirecaoDesvio, obstaculoCritico.AnguloParaDesvio, obstaculosClone, passoSimulacaoCm, ref passo);
                        simulacoes.Add(simulacaoAtual);
                    }
                    else
                    {
                        break; // Não há mais obstáculos a considerar
                    }
                }

                if (simulacaoAtual.RiscoTotal >= LimiteRiscoAceitavel || !obstaculosClone.Any())
                {
                    break;
                }

                obstaculosClone = simulacaoAtual.Obstaculos.ToList();
            }

            return simulacoes;
        }

        private SimulacaoDesvio SimularPasso(Direcao direcaoDesvio, double anguloDesvio, List<Obstaculo> obstaculosSimulados, float passoSimulacaoCm, ref int passo)
        {
            passo++;

            var obstaculosClone = obstaculosSimulados.Select(o => o.Clone()).ToList();
            double novoAnguloDesvio = double.IsNaN(anguloDesvio) ? 0 : anguloDesvio;

            // Conversão de ângulo para radianos para cálculos trigonométricos
            double anguloEmRadianos = novoAnguloDesvio * (Math.PI / 180.0);

            foreach (var obstaculo in obstaculosClone)
            {
                // Calcular o deslocamento em Y (distância até o robô) considerando o passo da simulação
                float deslocamentoYmm = (passoSimulacaoCm * 10); // Convertendo de cm para mm
                obstaculo.DistanciaMedia_mm -= deslocamentoYmm;

                // Calcular o deslocamento em X (lateral) com base no ângulo de desvio
                float deslocamentoXmm = Convert.ToInt32(deslocamentoYmm * Math.Tan(anguloEmRadianos));

                // Atualizar as novas dimensões do obstáculo com base na nova distância
                float ratioAtual = KinectService.CalculatePixelToMMRatio(obstaculo.DistanciaMedia_mm);
                float fatorCrescimento = KinectService.CalculatePixelToMMRatio(obstaculo.DistanciaMedia_mm + deslocamentoYmm) / ratioAtual;

                var b = ((obstaculo.Largura_mm * fatorCrescimento) - obstaculo.Largura_mm) / 2;
                int deslocamentoXpx = (int)(deslocamentoXmm / ratioAtual);
                var d = fatorCrescimento * obstaculo.Width;
                var e = (int)((d - obstaculo.Width));

                var c = ((obstaculo.Altura_mm * fatorCrescimento) - obstaculo.Altura_mm) / 2;
                var f = fatorCrescimento * obstaculo.Height;
                var g = (int)((f - obstaculo.Height) / 2);
                int deslocamentoYpx = (int)(deslocamentoYmm / ratioAtual);

                obstaculo.Y += (int)g;
                if (direcaoDesvio == Direcao.Esquerda)
                {
                    obstaculo.X += (int)(e / 2);
                }
                else if (direcaoDesvio == Direcao.Direita)
                {
                    obstaculo.X -= (int)(e * 2);
                }
                

                
                obstaculo.Width = Convert.ToInt32(obstaculo.Width * fatorCrescimento);
                obstaculo.Height = Convert.ToInt32(obstaculo.Height * fatorCrescimento);
            }

            //Bitmap bitmapSimulacao = (Bitmap)KinectService.bitmapRGB.Clone();
            //bitmapSimulacao = (Bitmap)KinectService.ExibirObstaculos(bitmapSimulacao, KinectService.Parametros.Subdivisoes, obstaculosClone, true).Clone();
            //bitmapSimulacao.Save($"C:\\simu\\{passo}.jpg");

            double riscoTotal = CalcularRiscoTotal(obstaculosClone);

            return new SimulacaoDesvio
            {
                DirecaoDesvio = direcaoDesvio,
                AnguloDesvio = novoAnguloDesvio, // Mantém o ângulo original até a ultrapassagem do obstáculo crítico
                RiscoTotal = riscoTotal,
                Obstaculos = obstaculosClone
            };
        }

        public KinectModel Clone()
        {
            return new KinectModel()
            {
                AccW = AccW,
                AccX = AccX,
                AccY = AccY,
                AccZ = AccZ,
                AnguloDesvio = AnguloDesvio, 
                AnguloElevacao = AnguloElevacao,
                Colisao = Colisao,
                DistanciaDaDireita = DistanciaDaDireita,
                DistanciaDaEsquerda = DistanciaDaEsquerda,
                Momento = Momento,
                Obstaculos = new List<Obstaculo>(Obstaculos),
                Pitch = Pitch, 
                Roll = Roll,
                Subdivisoes = new List<Subdivisao>(Subdivisoes ?? new List<Subdivisao>()),
                UltimoCalculo = UltimoCalculo,
                UltimoComandoRespondido = UltimoComandoRespondido,
                UltimoStatusDetecao = UltimoStatusDetecao, 
                VelX = VelX,
                VelY = VelY,
                VelZ = VelZ,
                Yaw = Yaw,
            };
        }
    }

    
    public class KinectParametrosModel
    {
        public int Angulo { get; set; }
        public float focalLength { get; set; }
        public float sensorWidthMM { get; set; }
        public int maxDepth { get; set; }
        public int minDepth { get; set; }
        public int distProjetarTrajetoriaMm { get; set; }
        public int depthFrameWidth { get; set; }
        public int depthFrameHeight { get; set; }
        public int toleranciaDeteccaoTrajetoria { get; set; }
        public int toleranciaDeteccaoSuperior { get; set; }
        public int subdivisoesX { get; set; }
        public int subdivisoesY { get; set; }
        public List<Subdivisao> Subdivisoes { get; set; } = new List<Subdivisao>();
        public List<Subdivisao> SubdivisoesTrajetoria
        {
            get
            {
                return Subdivisoes.Where(x => !x.IsZonaSuperior).ToList();
            }
        }
        public List<Subdivisao> SubdivisoesSuperior
        {
            get
            {
                return Subdivisoes.Where(x => x.IsZonaSuperior).ToList();
            }
        }
    }

    // Classe para armazenar informações sobre cada subdivisão
    

    public class SimulacaoDesvio
    {
        public Direcao DirecaoDesvio { get; set; }
        public double AnguloDesvio { get; set; }
        public double RiscoTotal { get; set; }
        public List<Obstaculo> Obstaculos { get; set; }
    }

}