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

namespace AgroBase.Models.Modules
{
    public class MovimentacaoUnificadaModel : DispositivoBaseModel
    {
        public bool Conectado
        {
            get
            {
                return Modulos.Any(x => x.Conectado);
            }
            set
            {

            }
        }

        public static int _TaxaAmostragem { get; set; } = 1000;
        
        public double DistanciaPercorridaOperacao { get; set; } = 0;
        public int TempoEmAtividade { get; set; } = 0;
        public double DistanciaPercorridaTotal { get; set; } = 0;
        public double DistanciaPercorridaParcial { get; set; } = 0;
        public double DiametroRoda { get; set; } = 0.3556;
        public double ReducaoMotorBLDC { get; set; } = 5;
        public double RPM_Max_MotorBLDC { get; set; } = 500;

        public string IP_Bld { get; set; } = Variaveis.FaixaIPethernet + ".10";
        public string IP_Mks { get; set; } = Variaveis.FaixaIPethernet + ".11";

        public static int LogsManter { get; set; } = 60;
        public AsyncTaskTimerModel tmrRegistrador;
        public AsyncTaskTimerModel tmrRegistradorBLD;
        public AsyncTaskTimerModel tmrRegistradorMKS;


        public List<ModuloMvdModel> Modulos { get; set; } = new List<ModuloMvdModel>();

        public static Direcao UltimaDirecao { get; set; } = Direcao.Parado;
        
        public (bool, List<string>) StatusModulosBLD(Direcao Lado) 
        {
            string Prefixo = Lado == Direcao.Esquerda ? "E" : Lado == Direcao.Direita ? "D" : "";
            var Mods = Modulos.Where(Mod => Mod.Modulo_ID.Contains(Prefixo) && Mod.MovMotor.Comandar).ToList();
            var ModsComFalha = Mods
                .Where(Mod => 
                    !Mod.MovMotor.BLD_Inicializado || 
                    Mod.MovMotor.CodigoAlarme != 0 || 
                    Mod.MovMotor.AlarmeSensores
                )
                .ToList();

            bool _operante = ModsComFalha.Count() < Mods.Count();
            List<string> ModsInoperantes = ModsComFalha.Select(x => x.Modulo_ID).ToList();

            return (_operante, ModsInoperantes);
        }

        public (bool, List<string>) StatusModulosMKS(TipoMovimentoDirecional TipoMovimento)
        {
            var Mods = Modulos.Where(Mod => Mod.DirMotor.Comandar).ToList();
            var ModsComFalha = Mods
                .Where(Mod => 
                    !Mod.DirMotor.MKS_Inicializado && (
                        Mod.DirMotor.ConfigSentidos[TipoMovimento].Esquerda != Sentido.Parado && 
                        Mod.DirMotor.ConfigSentidos[TipoMovimento].Direita != Sentido.Parado
                    )
                );

            bool _operante = !ModsComFalha.Any();
            List<string> ModsInoperantes = ModsComFalha.Select(x => x.Modulo_ID).ToList();

            return (_operante, ModsInoperantes);
        }


        public static MovimentacaoUnificadaModel CarregarParametrosIniciais()
        {
            return new MovimentacaoUnificadaModel()
            {
                Modulos = new List<ModuloMvdModel>()
                {
                    new ModuloMvdModel()
                    {
                        Modulo_ID = "ET",
                        MovMotor = MvdMotorBLDC.CarregarParametrosIniciais("ET", "0x01"),
                        DirMotor = MvdMotorPasso.CarregarParametrosIniciais("ET", "0x01"),
                    },
                    new ModuloMvdModel()
                    {
                        Modulo_ID = "EF",
                        MovMotor = MvdMotorBLDC.CarregarParametrosIniciais("EF", "0x03"),
                        DirMotor = MvdMotorPasso.CarregarParametrosIniciais("EF", "0x03")
                    },
                    new ModuloMvdModel()
                    {
                        Modulo_ID = "DT",
                        MovMotor = MvdMotorBLDC.CarregarParametrosIniciais("DT", "0x02"),
                        DirMotor = MvdMotorPasso.CarregarParametrosIniciais("DT", "0x02")
                    },
                    new ModuloMvdModel()
                    {
                        Modulo_ID = "DF",
                        MovMotor = MvdMotorBLDC.CarregarParametrosIniciais("DF", "0x04"),
                        DirMotor = MvdMotorPasso.CarregarParametrosIniciais("DF", "0x04")
                    },
                },
            };
        }

        public void LimparDados<T>(T _dados)
        {
            var Dados = _dados as MovimentacaoUnificadaModel;
            Dados.Modulos.ForEach(x =>
            {
                x.MovMotor.Leitura = null;
                x.DirMotor.Leitura = null;
            });

            ReiniciarLeituras(Dados);
        }

        public void ReiniciarLeituras(MovimentacaoUnificadaModel Dados)
        {
            Dados.DistanciaPercorridaParcial = 0;
            Dados.DistanciaPercorridaTotal = 0;
            Dados.DistanciaPercorridaOperacao = 0;
            Dados.TempoEmAtividade = 0;
            Dados.Modulos.ForEach(x =>
            {
                x.MovMotor.AlarmeSensores = false;
                x.MovMotor.ConfigFreio.Leitura = false;
                x.MovMotor.LogGrafico = new List<MvdMotorMOVSensorimanetoGrafico>();
                x.MovMotor.Temperatura = 0;
                x.MovMotor.RPM_SP = 0;
                x.MovMotor.RPM_SP_Controle = 0;
                x.MovMotor.Corrente = 0;

                x.DirMotor.Angulo_SP = 0;
                x.DirMotor.Sentido_SP = Sentido.Parado;
            });
        }

        public Dictionary<string, List<string>> ProtocoloConfiguracao(bool Conectar)
        {
            var Config = Modulos.Where(x => x.MovMotor.Comandar || x.DirMotor.Comandar).ToDictionary(x => x.Modulo_ID, x => x.ProtocoloConfiguracao(Conectar).ToList());
            return Config;
        }

        public string ProtocoloVerificacao(bool Completo)
        {
            return "";
        }

        public void RecebeProtocoloModulo(string Protocolo)
        {
            
        }

        public void RegistrarLogMov(ModuloMvdModel Modulo)
        {
            Modulo.MovMotor.LogGrafico.Add(new MvdMotorMOVSensorimanetoGrafico()
            {
                Momento = DateTime.Now,
                Modulo_ID = Modulo.Modulo_ID,
                RPM_SP = Modulo.MovMotor.RPM_SP_Leitura, 
                RPM_Motor = Modulo.MovMotor.RPM_Motor,
                RPM_Roda = Modulo.MovMotor.RPM_Roda, 
                Corrente = Modulo.MovMotor.Corrente,
                Sentido = Modulo.MovMotor.Sentido,
                Temperatura = Modulo.MovMotor.Temperatura,
                TemperaturaDriver = Modulo.MovMotor.TemperaturaDriver,
                Tensao = Modulo.MovMotor.Tensao,
                Velocidade = Modulo.MovMotor.VelocidadeInstantanea
            });

            if (Modulo.MovMotor.LogGrafico.Count > 60)
            {
                Modulo.MovMotor.LogGrafico.RemoveAt(0);
            }
        }

        public void DefineStatusConexao(string Mod_ID, bool conecatdo)
        {
            var Modulo = Modulos.FirstOrDefault(x => x.Modulo_ID == Mod_ID);
            if (Modulo != null)
            {
                //Modulo.Conectado = conecatdo;
            }
        }

        public string GetTaxaAmostragem()
        {
            return _TaxaAmostragem.ToString("00000");
        }

        public void SetTaxaAmostragem(int TaxaAmostragem)
        {
            _TaxaAmostragem = TaxaAmostragem;
        }

        public bool GetStatusConexao()
        {
            return Conectado;
        }

        public string GetIP()
        {
            return IP_Bld;
        }

        public void IniciarRegistrador()
        {
            if (!(tmrRegistrador?.IsRunning ?? false))
            {
                LimparDados(this);

                PararRegistrador();

                tmrRegistrador = new AsyncTaskTimerModel("tmrRegistrador_" + T_Code.Mvd.ToString(), tmrRegistrador_Tick, _TaxaAmostragem);
                tmrRegistrador.Start();

                tmrRegistradorBLD = new AsyncTaskTimerModel("tmrRegistrador_" + T_Code.Bld.ToString(), tmrRegistradorBLD_Tick, BLD300RService.TaxaAmostragem);
                tmrRegistradorBLD.Start();

                tmrRegistradorMKS = new AsyncTaskTimerModel("tmrRegistrador_" + T_Code.Mks.ToString(), tmrRegistradorMKS_Tick, MKS057DService.TaxaAmostragem);
                tmrRegistradorMKS.Start();
            }
        }

        public void PararRegistrador()
        {
            tmrRegistrador?.Dispose();
            tmrRegistradorBLD?.Dispose();
            tmrRegistradorMKS?.Dispose();
        }

        public async Task tmrRegistrador_Tick()
        {
            tmrRegistrador.SetInterval(_TaxaAmostragem);

            if (UltimaDirecao != Direcao.Parado)
            {
                TempoEmAtividade++;
                double VelocidadeMedia = Modulos.Where(x => x.Conectado && x.MovMotor.ComandoLigado).DefaultIfEmpty().Average(x => x?.MovMotor?.VelocidadeInstantanea ?? 0);
                VelocidadeMedia /= 3.6; // converter de km/h para m/s
                double tempo = _TaxaAmostragem / 1000;
                DistanciaPercorridaTotal += tempo * VelocidadeMedia;
                DistanciaPercorridaParcial += (UltimaDirecao == Direcao.Frente ? tempo : UltimaDirecao == Direcao.Tras ? -tempo : 0) * VelocidadeMedia;
                DistanciaPercorridaOperacao += tempo * VelocidadeMedia;
                Variaveis.OperacaoEmAndamento.Sensoriamento.VelocidadeMedia = VelocidadeMedia;
            }
        }

        private async Task tmrRegistradorBLD_Tick()
        {
            if (!BLD300RService.Iniciado && SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Bld))
            {
                await BLD300RService.InicializarEthernetService();

                foreach (var Leitura in BLD300RService.DadosLeitura.Where(x => x.UltimoComandoRecebido.AddSeconds(10) < DateTime.Now))
                {
                    BLD300RService.AdicionarComandoNaFila(new BLD300RService.BLDComandoModel()
                    {
                        Endereco = Leitura.Endereco,
                        Parametro = BLD300RService.Parametros.First(x => x.Chave == "Versao"),
                        Get = true,
                        AguardaResposta = true
                    });
                }

                tmrRegistradorBLD.SetInterval(3000);
                return;
            }

            var Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov);

            foreach (var Modulo in Modulos.Where(x => x.MovMotor.BLD_Inicializado).OrderBy(x => x.MovMotor.EnderecoByte))
            {
                foreach (var Parametro in BLD300RService.Parametros.Where(x => x.LeituraLoop))
                {
                    var Comando = new BLD300RService.BLDComandoModel()
                    {
                        Endereco = Modulo.MovMotor.EnderecoByte,
                        AguardaResposta = true,
                        Parametro = Parametro,
                        Get = true,
                        TimeoutResposta = 500
                    };
                    BLD300RService.AdicionarComandoNaFila(Comando);
                }

                Modulo.MovMotor.EnviarComandoControle(Controle.UltimaDirecao);

                Modulo.MovMotor.ConfigFreio.Leitura = Modulo.MovMotor.Leitura?.Freado ?? false;

                Variaveis.OperacaoEmAndamento.DispMvd.Dados.RegistrarLogMov(Modulo);
            }

            tmrRegistradorBLD.SetInterval(BLD300RService.TaxaAmostragem);
        }

        private async Task tmrRegistradorMKS_Tick()
        {
            if (!MKS057DService.Iniciado && SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Mks))
            {
                await MKS057DService.InicializarEthernetService();

                foreach (var Leitura in MKS057DService.DadosLeitura.Where(x => x.UltimoComandoRecebido.AddSeconds(10) < DateTime.Now))
                {
                    MKS057DService.AdicionarComandoNaFila(MKS057DService.MontarComandoRequisicaoPulsos(Leitura.Endereco));
                }

                tmrRegistradorMKS.SetInterval(3000);
                return;
            }

            var Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir);
            foreach (var Modulo in Modulos.Where(x => x.DirMotor.MKS_Inicializado))
            {
                byte modAddr = Modulo.DirMotor.EnderecoByte;
                if (!MKS057DService.Referenciando || (MKS057DService.Referenciando && MKS057DService.AddrRef == modAddr))
                {
                    bool reqParametros = true; // (!MKS057DService.Referenciando && (tmrRegistradorMKS.Ticks % 5 == 0)) || MKS057DService.Referenciando;
                    if (reqParametros)
                    {
                        MKS057DService.AdicionarComandoNaFila(MKS057DService.MontarComandoRequisicaoPulsos(modAddr));
                        MKS057DService.AdicionarComandoNaFila(MKS057DService.MontarComandoRequisicaoIO(modAddr));
                    }

                    // Verifica se o angulo de set point do motor em questao e diferente do angulo atual, e se for, reenvia o comando para garantir que o motor esteja coerente com o set point
                    if (!MKS057DService.Referenciando && (Modulo.DirMotor.UltimoComandoEnviado.AddMilliseconds(2000) < DateTime.Now) && !Modulo.DirMotor.AtingiuAnguloSP)
                    {
                        Modulo.DirMotor.EnviarComandoControle(Controle.UltimaDirecao, !Modulo.DirMotor.Comandar);
                    }
                }
            }

            MKS057DService.TaxaAmostragem = MKS057DService.Referenciando ? 200 : 1000;
            tmrRegistradorMKS.SetInterval(MKS057DService.TaxaAmostragem);
        }



        public async Task ExecutarMovimentoDirecionalCombinado(Direcao DirecaoAtual)
        {
            if (DirecaoAtual == Direcao.Parado)
            {
                foreach (var Modulo in Modulos)
                {
                    Modulo.MovMotor.EnviarComandoControle(Direcao.Parado);
                }

                await Task.Delay(1000);
            }

            switch (Variaveis.OperacaoEmAndamento.Controle.TipoMovimento)
            {
                case TipoMovimentoDirecional.RotacionarNoEixo:
                case TipoMovimentoDirecional.MovimentoLateral:

                    foreach (var Modulo in Modulos.Where(x => x.DirMotor.MKS_Inicializado && !x.DirMotor.PosicaoTipoMovimentoConcluido(DirecaoAtual)))
                    {
                        Modulo.DirMotor.EnviarComandoControle(DirecaoAtual);
                    }
                    
                    DateTime Agora = DateTime.Now;
                    bool Sucesso = Modulos.All(x => x.DirMotor.PosicaoTipoMovimentoConcluido(DirecaoAtual));
                    while (Agora.AddMilliseconds(5000) > DateTime.Now && !Sucesso)
                    {
                        await Task.Delay(100);
                        Sucesso = Modulos.All(x => x.DirMotor.PosicaoTipoMovimentoConcluido(DirecaoAtual));
                    }

                    if (Sucesso && DirecaoAtual != Direcao.Parado)
                    {
                        Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP = 20;
                        foreach (var Modulo in Modulos)
                        {
                            Modulo.MovMotor.EnviarComandoControle(
                                DirecaoAtual == Direcao.Direita ? Direcao.Frente :
                                DirecaoAtual == Direcao.Esquerda ? Direcao.Tras : 
                                Direcao.Parado
                            );
                        }
                    }

                    break;

            }
        }

        public async Task RealizarReferenciamentoDirecional()
        {
            if (!MKS057DService.Referenciando)
            {
                TipoMovimentoDirecional tipoControle = Variaveis.OperacaoEmAndamento.Controle.TipoMovimento;

                Variaveis.OperacaoEmAndamento.Controle.TipoMovimento = TipoMovimentoDirecional.Diagnostico;
                foreach (var Modulo in Modulos.Where(x => x.DirMotor.Comandar && x.DirMotor.MKS_Inicializado && !x.DirMotor.IN1_Atuado))
                {
                    await Modulo.DirMotor.ReferenciaMotor();
                }
                Variaveis.OperacaoEmAndamento.Controle.TipoMovimento = tipoControle;
            }
        }



    }

    public class ModuloMvdModel
    {
        public string Modulo_ID { get; set; }
        public bool Disponivel { get; set; }
        public bool RequisitarDados { get; set; } = false;

        public bool Conectado
        {
            get
            {
                return MovMotor.BLD_Inicializado || DirMotor.MKS_Inicializado;
            }
        }

        public MvdMotorBLDC MovMotor { get; set; }
        public MvdMotorPasso DirMotor { get; set; }

        public static PinoutDataModel Pinout { get; set; }
        public static List<FuncoesPinout> Funcoes { get; set; } = new List<FuncoesPinout>()
        {
            FuncoesPinout.TX,
            FuncoesPinout.RX,
            FuncoesPinout.ModbusDI,
            FuncoesPinout.ModbusRO,
            FuncoesPinout.ModbusDE_RE
        };


        public List<string> ProtocoloConfiguracao(bool Conectar)
        {
            List<string> Config = new List<string>();

            var _pinout = new PinoutDataModel()
            {
                ExpansorGPIO = Pinout.ExpansorGPIO,
                EnderecoExpansor = Pinout.EnderecoExpansor,
                Pinos = Pinout.Pinos.ToList(),
            };
            _pinout.Pinos = _pinout.Pinos.Where(x => x.ComponenteID == T_Code.Mvd.ToString()).ToList();
            string ProtocoloMOD =
                ((int)F_Code.Cfg).ToString() + SerialService.SplitMessage +
                Modulo_ID + SerialService.SplitParams +
                (Conectar ? "1" : "0") + SerialService.SplitParams +
                MovimentacaoUnificadaModel._TaxaAmostragem.ToString() + SerialService.SplitParams +
                (RequisitarDados ? "1" : "0") + SerialService.SplitParams +
                PinoutModel.PinoProtocolo(_pinout, Funcoes, SerialService.SplitParams);

            if (Conectar)
            {
                Config.Add(ProtocoloMOD);
            }

            if (MovMotor.Comandar)
            {
                Config.Add(MovMotor.ProtocoloConfiguracao(Conectar));
            }
            /*if (DirMotor.Comandar)
            {
                Config.Add(DirMotor.ProtocoloConfiguracao(Conectar));
            }*/

            if (!Conectar)
            {
                Config.Add(ProtocoloMOD);
            }

            Config.Add(ProtocoloVerificacao(false));

            return Config;
        }

        public string ProtocoloVerificacao(bool Completo)
        {
            if (Completo)
            {
                string ProtocoloCheck =
                    SerialService.BeginLine +
                    0 + SerialService.SplitMessage +
                    Modulo_ID + SerialService.SplitMessage +
                    ((int)F_Code.Chk).ToString() + SerialService.SplitMessage +
                    "check" +
                    SerialService.EndLine;

                return ProtocoloCheck;
            }
            else
            {
                string ProtocoloCheck = 
                    ((int)F_Code.Chk).ToString() + SerialService.SplitMessage +
                    "check";

                return ProtocoloCheck;
            }
        }

        public void AtualizarSensoresMotor()
        {
            var sensorTmp = Variaveis.OperacaoEmAndamento.DispSen.Dados.DadosLeitura.SensoresTemperatura.FirstOrDefault(x => x.ID.Contains(Modulo_ID));
            if (sensorTmp != null)
            {
                MovMotor.Temperatura = sensorTmp.Temperatura;

                AlarmeModel _Alarme = new AlarmeModel()
                {
                    Tipo = S_Code.sTMP,
                    Valor = sensorTmp.Temperatura,
                    Maximo = sensorTmp.Temperatura > sensorTmp.LimiteMaximo,
                    Minimo = sensorTmp.Temperatura < sensorTmp.LimiteMinimo,
                    ParaMaximo = false,
                    ParaMinimo = false
                };
                VerificarAlarme(_Alarme);
            }
            var sensorCor = Variaveis.OperacaoEmAndamento.DispSen.Dados.DadosLeitura.SensoresCorrente.FirstOrDefault(x => x.ID.Contains(Modulo_ID));
            if (sensorCor != null)
            {
                MovMotor.Corrente = (double)sensorCor.Current;

                AlarmeModel _Alarme = new AlarmeModel()
                {
                    Tipo = S_Code.sCOR,
                    Valor = (double)sensorCor.Current,
                    Maximo = (double)sensorCor.Current > sensorCor.LimiteMaximo,
                    Minimo = (double)sensorCor.Current < sensorCor.LimiteMinimo,
                    ParaMaximo = false,
                    ParaMinimo = false
                };
                VerificarAlarme(_Alarme);
            }
        }

        private void VerificarAlarme(AlarmeModel _Alarme)
        {
            bool Adicionado = false;
            var Alm = MovMotor.Alarmes.FirstOrDefault(x => x.Tipo == _Alarme.Tipo);
            if (Alm == null)
            {
                Alm = _Alarme;
                MovMotor.Alarmes.Add(Alm);
                Adicionado = true;
            }

            bool AlarmeAnterior = (Alm.Maximo && Alm.ParaMaximo) || (Alm.Minimo && Alm.ParaMinimo);
            bool AlarmeAtual = (_Alarme.Maximo && _Alarme.ParaMaximo) || (_Alarme.Minimo && _Alarme.ParaMinimo);

            if (MovMotor.BLD_Inicializado && (!AlarmeAnterior || Adicionado) && AlarmeAtual) // Motor iniciado, Não estava em alarme anteriormente ou foi adicionado agora, alarme atual
            {
                MovMotor.EnviarComandoControle(Direcao.Parado);
            }

            Alm.Valor = _Alarme.Valor;
            Alm.Maximo = _Alarme.Maximo;
            Alm.Minimo = _Alarme.Minimo;

            MovMotor.AlarmeSensores = MovMotor.Alarmes.Any(x => (x.Maximo && x.ParaMaximo) || (x.Minimo && x.ParaMinimo));
        }

    }

    public class MvdMotorBLDC
    {
        public string ID { get; set; } = T_Code.Mov.ToString();

        // SETs
        public string Endereco { get; set; }
        public byte EnderecoByte
        {
            get
            {
                return Convert.ToByte(Endereco.Replace("0x", ""), 16);
            }
        }
        public bool LeituraLigado
        {
            get
            {
                return Leitura?.Ativado ?? false;
            }
        }
        public bool ComandoLigado { get; set; } = false;
        //public bool Freio { get; set; } = false;
        public int CodigoAlarme
        {
            get
            {
                return Leitura?.CodigoAlarme ?? 0;
            }
        }
        public bool AlarmeMotor
        {
            get
            {
                return CodigoAlarme > 0 && CodigoAlarme < 99;
            }
        }
        public int RPM_SP { get; set; } = 0;
        public int RPM_SP_Controle { get; set; } = 0;
        public int RPM_SP_Leitura
        {
            get
            {
                return RPM_SP / Convert.ToInt32(Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.ReducaoMotorBLDC ?? 1);
            }
        }
        public Sentido Sentido_SP { get; set; } = Sentido.Parado;
        public int NumeroPolos { get; set; } = 10;
        public int Rampa { get; set; } = 0;
        public double OffsetLeitura { get; set; } = 1.0;


        // GETs
        public double RPM_Roda
        {
            get
            {
                return (Leitura?.RPM ?? 0) * OffsetLeitura;
            }
        }
        public Sentido Sentido
        {
            get
            {
                return Leitura?.Direcao ?? Sentido.Parado;
            }
        }
        public double Temperatura { get; set; } = 0;
        public double TemperaturaDriver
        {
            get
            {
                return Math.Round(Leitura?.Temperatura ?? 0, 2);
            }
        }
        public double Tensao
        {
            get
            {
                return Math.Round(Leitura?.Tensao ?? 0, 2);
            }
        }
        public double Corrente_Max
        {
            get
            {
                return Math.Round(Leitura?.CorrenteMax ?? 0, 2);
            }
        }
        public double Corrente { get; set; } = 0;
        public double RPM_Motor
        {
            get
            {
                //return Convert.ToInt32(RPM_Motor / VariaveisEquipamento.RelacaoRPM);
                return RPM_Roda * VariaveisEquipamento.RelacaoRPM;
                //return RPM_Roda * 0.74;
            }
        }
        public double VelocidadeInstantanea
        {
            get
            {
                return FuncoesMatematicas.CalculaVelocidadeRPM(RPM_Motor);
            }
        }


        public bool Comandar { get; set; } = false;
        public bool AlarmeSensores { get; set; } = false;
        public bool BLD_Inicializado
        {
            get
            {
                return Leitura?.Iniciado ?? false;
            }
        }
        private BLD300RModel _leitura = null;
        public BLD300RModel Leitura
        {
            get
            {
                _leitura = BLD300RService.DadosLeitura?.FirstOrDefault(x => x.Endereco == EnderecoByte);
                return _leitura;
            }
            set
            {
                _leitura = value;
            }
        }


        public Dictionary<TipoMovimentoDirecional,ConfiguracaoSentidoMotor> ConfigSentidos { get; set; }
        public List<FuncoesPinout> Funcoes { get; set; }

        public List<MvdMotorMOVSensorimanetoGrafico> LogGrafico { get; set; } = new List<MvdMotorMOVSensorimanetoGrafico>();
        public List<AlarmeModel> Alarmes { get; set; } = new List<AlarmeModel>();

        public MvdServoFreioModel ConfigFreio { get; set; } = new MvdServoFreioModel();


        public static MvdMotorBLDC CarregarParametrosIniciais(string Mod_ID, string _Endereco)
        {
            return new MvdMotorBLDC()
            {
                ID = T_Code.Mov.ToString(),
                Endereco = _Endereco,
                Funcoes = new List<FuncoesPinout>()
                {
                    FuncoesPinout.TMP, 
                    FuncoesPinout.ENA,
                    FuncoesPinout.DIR,
                    FuncoesPinout.PWM,
                    FuncoesPinout.BRK,
                },
                ConfigFreio = new MvdServoFreioModel()
                {
                    AnguloInicial = 10, 
                    Incremental = Mod_ID.Contains("E"), 
                    Leitura = false, 
                },
                ConfigSentidos = new Dictionary<TipoMovimentoDirecional, ConfiguracaoSentidoMotor>()
                {
                    {
                        TipoMovimentoDirecional.RodasDianteiras,
                        new ConfiguracaoSentidoMotor()
                        {
                            Direita = Sentido.Parado,
                            Esquerda = Sentido.Parado,
                            Frente = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
                            Tras = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario
                        }
                    },
                    {
                        TipoMovimentoDirecional.RodasTraseiras,
                        new ConfiguracaoSentidoMotor()
                        {
                            Direita = Sentido.Parado,
                            Esquerda = Sentido.Parado,
                            Frente = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
                            Tras = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario
                        }
                    },
                    {
                        TipoMovimentoDirecional.MovimentoDiagonal,
                        new ConfiguracaoSentidoMotor()
                        {
                            Direita = Sentido.Parado,
                            Esquerda = Sentido.Parado,
                            Frente = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
                            Tras = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario
                        }
                    },
                    {
                        TipoMovimentoDirecional.MovimentoArco,
                        new ConfiguracaoSentidoMotor()
                        {
                            Direita = Sentido.Parado,
                            Esquerda = Sentido.Parado,
                            Frente = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
                            Tras = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario
                        }
                    },
                    {
                        TipoMovimentoDirecional.RotacionarNoEixo,
                        new ConfiguracaoSentidoMotor()
                        {
                            Direita = Sentido.Parado,
                            Esquerda = Sentido.Parado,
                            Frente = Sentido.Horario,
                            Tras = Sentido.Antihorario
                        }
                    },
                    {
                        TipoMovimentoDirecional.MovimentoLateral,
                        new ConfiguracaoSentidoMotor()
                        {
                            Direita = Sentido.Parado,
                            Esquerda = Sentido.Parado,
                            Frente = Mod_ID.Contains("T") ? Sentido.Horario : Sentido.Antihorario,
                            Tras = Mod_ID.Contains("T") ? Sentido.Antihorario : Sentido.Horario
                        }
                    },
                    {
                        TipoMovimentoDirecional.Diagnostico,
                        new ConfiguracaoSentidoMotor()
                        {
                            Direita = Sentido.Parado,
                            Esquerda = Sentido.Parado,
                            Frente = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
                            Tras = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario
                        }
                    },
                },
            };
        }


        public string ProtocoloConfiguracao(bool Conectar)
        {
            var _pinout = new PinoutDataModel()
            {
                ExpansorGPIO = ModuloMvdModel.Pinout.ExpansorGPIO,
                EnderecoExpansor = ModuloMvdModel.Pinout.EnderecoExpansor,
                Pinos = ModuloMvdModel.Pinout.Pinos.ToList(),
            };
            _pinout.Pinos = _pinout.Pinos.Where(x => x.ComponenteID == ID).ToList();

            string Protocolo =
                ((int)F_Code.Cfg).ToString() + SerialService.SplitMessage +
                ID + SerialService.SplitParams +
                (Conectar ? "1" : "0") + SerialService.SplitParams +
                Endereco + SerialService.SplitParams +
                NumeroPolos + SerialService.SplitParams +
                Rampa + SerialService.SplitParams +
                Variaveis.OperacaoEmAndamento.DispMvd.Dados.RPM_Max_MotorBLDC + SerialService.SplitParams +
                PinoutModel.PinoProtocolo(_pinout, Funcoes, SerialService.SplitParams);

            return Protocolo;
        }

        public void EnviarComandoControle(Direcao DirecaoAtual)
        {
            bool Freado = DirecaoAtual == Direcao.EmFreio;
            (bool AtuarServo, int AnguloFreio) = ConfigFreio.AtualizarDadosFreio(Freado, 0, -1, 5);

            if (Comandar && BLD_Inicializado)
            {
                if (AlarmeSensores || AlarmeMotor)
                {
                    DirecaoAtual = Direcao.Parado;
                    Freado = false;
                    (AtuarServo, AnguloFreio) = ConfigFreio.AtualizarDadosFreio(Freado, 0, -1, 5);

                    if (AlarmeMotor)
                    {
                        BLD300RService.RegisrarErrosComunicacao(EnderecoByte, $"Módulo com código de alarme {CodigoAlarme}, reiniciando o módulo...");
                        BLD300RService.AdicionarComandoNaFila(new BLD300RService.BLDComandoModel()
                        {
                            Endereco = EnderecoByte,
                            AguardaResposta = true,
                            TimeoutResposta = 500,
                            Get = false,
                            Parametro = BLD300RService.Parametros.First(x => x.Chave == "AlarmeCode"),
                            Valor = 0
                        });
                    }
                }
                else
                {
                    MovimentacaoUnificadaModel.UltimaDirecao = DirecaoAtual;
                }

                RPM_SP_Controle = Variaveis.OperacaoEmAndamento.Controle.RPM_Motor;
                RPM_SP =
                    DirecaoAtual == Direcao.Parado || Freado ? 0 :
                    RPM_SP_Controle;

                ComandoLigado = RPM_SP > 0;

                switch (DirecaoAtual)
                {
                    case Direcao.Frente:
                        Sentido_SP = ConfigSentidos[Variaveis.OperacaoEmAndamento.Controle.TipoMovimento].Frente;
                        break;
                    case Direcao.Tras:
                        Sentido_SP = ConfigSentidos[Variaveis.OperacaoEmAndamento.Controle.TipoMovimento].Tras;
                        break;
                    default:
                        Sentido_SP = Sentido.Parado;
                        break;
                }

                byte Addr = EnderecoByte;
                DateTime Agora = DateTime.Now;

                if (Sentido_SP != Sentido.Parado && (Leitura == null || Leitura.Direcao != Sentido_SP))
                {
                    BLD300RService.AdicionarComandoNaFila(new BLD300RService.BLDComandoModel()
                    {
                        Endereco = Addr,
                        Parametro = BLD300RService.Parametros.First(x => x.Chave == "Direction"),
                        Valor = Sentido_SP == Sentido.Horario ? 1 : 0,
                        Get = false,
                        AguardaResposta = true,
                        TimeoutResposta = 500,
                    });
                }
                if (ComandoLigado && (Leitura == null || Leitura.RPM_SP != RPM_SP_Controle))
                {
                    BLD300RService.AdicionarComandoNaFila(new BLD300RService.BLDComandoModel()
                    {
                        Endereco = Addr,
                        Parametro = BLD300RService.Parametros.First(x => x.Chave == "RPM_SP"),
                        Valor = RPM_SP,
                        Get = false,
                        AguardaResposta = true,
                        TimeoutResposta = 500,
                    });
                }
                if (Leitura == null || Leitura.Freado != Freado)
                {
                    BLD300RService.AdicionarComandoNaFila(new BLD300RService.BLDComandoModel()
                    {
                        Endereco = Addr,
                        Parametro = BLD300RService.Parametros.First(x => x.Chave == "Breaked"),
                        Valor = Freado ? 1 : 0,
                        Get = false,
                        AguardaResposta = true,
                        TimeoutResposta = 500,
                    });
                }
                if (Leitura == null || ComandoLigado != LeituraLigado)
                {
                    BLD300RService.AdicionarComandoNaFila(new BLD300RService.BLDComandoModel()
                    {
                        Endereco = Addr,
                        Parametro = BLD300RService.Parametros.First(x => x.Chave == "Enabled"),
                        Valor = ComandoLigado ? 1 : 0,
                        Get = false,
                        AguardaResposta = true,
                        TimeoutResposta = 500,
                    });
                }
            }

            if (AtuarServo)
            {
                string Mod_ID = Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.FirstOrDefault(x => x.MovMotor.Endereco == Endereco)?.Modulo_ID ?? "";
                var ServoFreio = Variaveis.OperacaoEmAndamento.DispSen.Dados.Servos.FirstOrDefault(x => x.ID.Contains(Mod_ID) && x.Inicializado && x.Controlar);
                if (ServoFreio != null)
                {
                    GeneralJoystick.EnviarComandoSensoriamento(S_Code.sFRO, ServoFreio.ID, AnguloFreio);
                }
            }
        }



    }

    public class MvdMotorPasso
    {
        public string ID { get; set; } = T_Code.Dir.ToString();

        // SETs
        public string Endereco { get; set; }
        public byte EnderecoByte
        {
            get
            {
                return Convert.ToByte(Endereco.Replace("0x", ""), 16);
            }
        }
        public Sentido Sentido_SP { get; set; } = Sentido.Parado;
        public double Angulo_SP { get; set; } = 0;
        public double Angulo_Offset { get; set; } = 0;
        public double Velocidade_Max { get; set; } = 600;
        public int Velocidade
        {
            get
            {
                return Convert.ToInt32(((Variaveis.OperacaoEmAndamento.Controle?.VelocidadeMP ?? 0) / 100.0) * Velocidade_Max);
            }
        }

        // GETs
        public Sentido Sentido
        {
            get
            {
                return Leitura?.Direcao ?? Sentido.Parado;
            }
        }
        public double Angulo
        {
            get
            {
                return MKS057DService.PulsosParaAngulo(Leitura?.NumeroPulsos ?? 0);
            }
        }
        public double RPM
        {
            get
            {
                return Leitura?.RPM ?? 0;
            }
        }
        public bool IN1_Atuado
        {
            get
            {
                return !(Leitura?.Entradas?[0] ?? true);
            }
        }
        public bool IN2_Atuado
        {
            get
            {
                return !(Leitura?.Entradas?[1] ?? true);
            }
        }


        public bool Comandar { get; set; } = false;
        public bool MKS_Inicializado
        {
            get
            {
                return Leitura?.Iniciado ?? false;
            }
        }
        private MKS057DModel _leitura = null;
        public MKS057DModel Leitura
        {
            get
            {
                _leitura = MKS057DService.DadosLeitura?.FirstOrDefault(x => x.Endereco == EnderecoByte);
                return _leitura;
            }
            set
            {
                _leitura = value;
            }
        }

        public Direcao UltimaDirecao { get; set; } = Direcao.Parado;
        public DateTime UltimoComandoEnviado { get; set; } = DateTime.MinValue;
        public bool RetornandoAzero
        {
            get
            {
                return (UltimaDirecao != Direcao.Esquerda && UltimaDirecao != Direcao.Direita);
            }
        }
        public bool EmMovimento
        {
            get
            {
                return RPM != 0;
            }
        }
        public bool AtingiuAnguloSP
        {
            get
            {
                double anguloSp = Comandar ? Angulo_SP : 0;
                return FuncoesMatematicas.ValorEstaEntre(Angulo, anguloSp, 1);
            }
        }

        public bool PosicaoTipoMovimentoConcluido(Direcao DirecaoAtual)
        {
            var TipoMovimento = Variaveis.OperacaoEmAndamento.Controle.TipoMovimento;
            switch (TipoMovimento)
            {
                case TipoMovimentoDirecional.RotacionarNoEixo:
                case TipoMovimentoDirecional.MovimentoLateral:
                    int agSP = DirecaoAtual != Direcao.Parado ? VariaveisEquipamento.AnguloMovimento[TipoMovimento] : 0;
                    bool anguloCorreto = agSP == Angulo;
                    if (!anguloCorreto && agSP != 0)
                    {
                        if (agSP > 0)
                        {
                            anguloCorreto = Angulo >= agSP;
                        }
                        else
                        {
                            anguloCorreto = Angulo <= agSP;
                        }
                    }
                    return !Comandar || anguloCorreto;
                default:
                    return !Comandar || Angulo_SP == Angulo;
            }
        }


        public Dictionary<TipoMovimentoDirecional, ConfiguracaoSentidoMotor> ConfigSentidos { get; set; }
        public List<FuncoesPinout> Funcoes { get; set; }


        public static MvdMotorPasso CarregarParametrosIniciais(string Mod_ID, string _Endereco)
        {
            return new MvdMotorPasso()
            {
                ID = T_Code.Dir.ToString(),
                Endereco = _Endereco, 
                Funcoes = new List<FuncoesPinout>()
                {
                    FuncoesPinout.PUL,
                    FuncoesPinout.DIR,
                    FuncoesPinout.ENA,
                    FuncoesPinout.EncoderAbsoluto
                },
                ConfigSentidos = new Dictionary<TipoMovimentoDirecional, ConfiguracaoSentidoMotor>()
                {
                    {
                        TipoMovimentoDirecional.RodasDianteiras,
                        new ConfiguracaoSentidoMotor()
                        {
                            Direita = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
                            Esquerda = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario,
                            Frente = Sentido.Parado,
                            Tras = Sentido.Parado
                        }
                    },
                    {
                        TipoMovimentoDirecional.RodasTraseiras,
                        new ConfiguracaoSentidoMotor()
                        {
                            Direita = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
                            Esquerda = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario,
                            Frente = Sentido.Parado,
                            Tras = Sentido.Parado
                        }
                    },
                    {
                        TipoMovimentoDirecional.MovimentoDiagonal,
                        new ConfiguracaoSentidoMotor()
                        {
                            Direita = Mod_ID == "EF" || Mod_ID == "DT" ? Sentido.Horario : Sentido.Antihorario,
                            Esquerda = Mod_ID == "EF" || Mod_ID == "DT" ? Sentido.Antihorario : Sentido.Horario,
                            Frente = Sentido.Parado,
                            Tras = Sentido.Parado
                        }
                    },
                    {
                        TipoMovimentoDirecional.MovimentoArco,
                        new ConfiguracaoSentidoMotor()
                        {
                            Direita = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
                            Esquerda = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario,
                            Frente = Sentido.Parado,
                            Tras = Sentido.Parado
                        }
                    },
                    {
                        TipoMovimentoDirecional.RotacionarNoEixo,
                        new ConfiguracaoSentidoMotor()
                        {
                            Direita = Sentido.Horario,
                            Esquerda = Sentido.Horario,
                            Frente = Sentido.Parado,
                            Tras = Sentido.Parado
                        }
                    },
                    {
                        TipoMovimentoDirecional.MovimentoLateral,
                        new ConfiguracaoSentidoMotor()
                        {
                            Direita = Mod_ID == "EF" || Mod_ID == "DT" ? Sentido.Horario : Sentido.Antihorario,
                            Esquerda = Mod_ID == "EF" || Mod_ID == "DT" ? Sentido.Antihorario : Sentido.Horario,
                            Frente = Sentido.Parado,
                            Tras = Sentido.Parado
                        }
                    },
                    {
                        TipoMovimentoDirecional.Diagnostico,
                        new ConfiguracaoSentidoMotor()
                        {
                            Direita = Sentido.Horario,
                            Esquerda = Sentido.Antihorario,
                            Frente = Sentido.Parado,
                            Tras = Sentido.Parado
                        }
                    },
                }
            };
        }

        public void EnviarComandoControle(Direcao Direcao, bool Forcar = false)
        {
            if (MKS_Inicializado && (Comandar || Forcar))
            {
                // Se o motor estiver nao utilizado mas nao estiver no angulo zero
                if (!Comandar)
                {
                    Angulo_SP = 0;
                    Sentido_SP = Sentido.Parado;
                }
                else
                {
                    int AnguloTipoMovimento = VariaveisEquipamento.AnguloMovimento[Variaveis.OperacaoEmAndamento.Controle.TipoMovimento];

                    if (UltimaDirecao != Direcao)
                    {
                        Sentido_SP =
                            Direcao == Direcao.Direita ? ConfigSentidos[Variaveis.OperacaoEmAndamento.Controle.TipoMovimento].Direita :
                            Direcao == Direcao.Esquerda ? ConfigSentidos[Variaveis.OperacaoEmAndamento.Controle.TipoMovimento].Esquerda :
                            Sentido_SP == Sentido.Horario ? Sentido.Antihorario :
                            Sentido_SP == Sentido.Antihorario ? Sentido.Horario :
                            Sentido.Parado;

                        UltimaDirecao = Direcao;
                    }
                    else if (AtingiuAnguloSP && RetornandoAzero && Angulo == 0)
                    {
                        Sentido_SP = Sentido.Parado;
                    }

                    Angulo_SP =
                        RetornandoAzero ? 0 :
                        AnguloTipoMovimento != -1 ? AnguloTipoMovimento :
                        Variaveis.OperacaoEmAndamento.Controle.Angulo;

                    int mx = Sentido_SP == Sentido.Antihorario ? -1 : 1;

                    Angulo_SP = Math.Abs(Angulo_SP) * mx;
                }
                
                if (true || !AtingiuAnguloSP)
                {
                    MKS057DService.AdicionarComandoNaFila(MKS057DService.MontarComandoMovimento(EnderecoByte, Angulo_SP, Velocidade, Sentido_SP));
                    UltimoComandoEnviado = DateTime.Now;
                }
            }
        }

        /*public async Task EnviarComandoControleAsync(Direcao Direcao)
        {
            if (Comandar && MKS_Inicializado)
            {
                int AnguloTipoMovimento = VariaveisEquipamento.AnguloMovimento[Variaveis.OperacaoEmAndamento.Controle.TipoMovimento];

                if (UltimaDirecao != Direcao)
                {
                    Sentido_SP =
                        Direcao == Direcao.Direita ? ConfigSentidos[Variaveis.OperacaoEmAndamento.Controle.TipoMovimento].Direita :
                        Direcao == Direcao.Esquerda ? ConfigSentidos[Variaveis.OperacaoEmAndamento.Controle.TipoMovimento].Esquerda :
                        Sentido_SP == Sentido.Horario ? Sentido.Antihorario :
                        Sentido_SP == Sentido.Antihorario ? Sentido.Horario :
                        Sentido.Parado;

                    UltimaDirecao = Direcao;
                }
                else if (AtingiuAnguloSP && RetornandoAzero && Angulo == 0)
                {
                    Sentido_SP = Sentido.Parado;
                }

                Angulo_SP =
                    RetornandoAzero ? 0 :
                    AnguloTipoMovimento != -1 ? AnguloTipoMovimento :
                    Variaveis.OperacaoEmAndamento.Controle.Angulo;

                int mx = Sentido_SP == Sentido.Antihorario ? -1 : 1;

                Angulo_SP = Math.Abs(Angulo_SP) * mx;

                if (!AtingiuAnguloSP || true)
                {
                    //await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoMovimento(EnderecoByte, Angulo_SP, Velocidade, Sentido_SP));
                    MKS057DService.AdicionarComandoNaFila(MKS057DService.MontarComandoMovimento(EnderecoByte, Angulo_SP, Velocidade, Sentido_SP));
                    UltimoComandoEnviado = DateTime.Now;
                }
            }
        }*/

        public async Task ReferenciaMotor()
        {
            byte Addr = EnderecoByte;

            MKS057DService.AddrRef = Addr;
            MKS057DService.UltimoReferenciamento = DateTime.Now;

            await Task.Delay(2000);

            await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoSetZero(Addr));

            await Task.Delay(500);

            bool Sucesso = false;
            bool EstadoInicial = IN1_Atuado;
            bool EstadoAnterior = EstadoInicial;

            double _anguloAnterior = Angulo;
            double _velocidadeAnterior = 0;
            int _angulo = 45;
            int _offset = 0;
            Sentido _sentido = Sentido.Horario;
            int tentativas = 0;
            double _anguloEntrada = 9999;
            double _anguloSaida = 9999;

            const int MaxTentativas = 10;
            DateTime TimeoutGeral = DateTime.Now.AddMinutes(2); // Timeout geral para o processo.

            double anguloMaxVisitado = 0;
            double anguloMinVisitado = 0;

            if (!Sucesso)
            {
                while (!Sucesso && tentativas < MaxTentativas && DateTime.Now < TimeoutGeral && MKS057DService.Referenciando)
                {
                    // Se o angulo ja foi visitado, entao pode se mover mais rapido, senao, se move mais devagar
                    int VelocidadeGiro = DefineVelocidadeReferenciamento(EstadoInicial, anguloMaxVisitado, anguloMinVisitado);

                    await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoMovimento(Addr, _angulo, VelocidadeGiro, _sentido));

                    DateTime Inicio = DateTime.Now;
                    bool inverterGiro = false;
                    while (!Sucesso && !inverterGiro && Inicio.AddSeconds(120) > DateTime.Now && MKS057DService.Referenciando)
                    {
                        bool _ref = IN1_Atuado;

                        //Sucesso = (EstadoInicial && !_ref) || (!EstadoInicial && _ref);

                        // Comecou com o sensor ja atuado
                        if (EstadoInicial)
                        {
                            // Quando houver alteracao na leitura do sensor
                            if (_ref != EstadoAnterior)
                            {
                                // Se estava atuado e agora nao esta mais, e ainda nao definiu o angulo de entrada
                                if (!_ref && _anguloEntrada == 9999)
                                {
                                    _anguloEntrada = Angulo;
                                    inverterGiro = true;
                                }
                                // Se estava atuado e agora nao esta mais, e ja definiu o angulo de entrada, e ainda nao definiu o angulo de saida
                                else if (!_ref && _anguloEntrada != 9999 && _anguloSaida == 9999)
                                {
                                    _anguloSaida = Angulo;
                                }
                            }
                        }
                        // Comecou com o sensor nao atuado
                        else
                        {
                            // Quando houver alteracao na leitura do sensor
                            if (_ref != EstadoAnterior)
                            {
                                // Se nao estava atuado e agora esta, e ainda nao definiu o angulo de entrada
                                if (_ref && _anguloEntrada == 9999)
                                {
                                    _anguloEntrada = Angulo;
                                }
                                // Se estava atuado e agora nao esta mais, e ja definiu o angulo de entrada, e ainda nao definiu o angulo de saida
                                else if (!_ref && _anguloEntrada != 9999 && _anguloSaida == 9999)
                                {
                                    _anguloSaida = Angulo;
                                    inverterGiro = true;
                                }
                            }
                        }

                        // Realiza a verificacao de necessidade de reverter a rotacao do motor apenas se ainda nao encontrou o angulo de entrada do sensor
                        if (_anguloEntrada == 9999)
                        {
                            inverterGiro = (_angulo > 0 && Angulo >= (_angulo - 0.5)) || (_angulo < 0 && Angulo <= (_angulo + 0.5));
                        }

                        Sucesso = _anguloEntrada != 9999 && _anguloSaida != 9999;

                        EstadoAnterior = _ref;

                        Console.WriteLine($"Ref {Addr}: Angulo SP = {_angulo}, Velocidade: {VelocidadeGiro}, Angulo Atual = {Angulo}, Sentido SP = {_sentido.ToString()}, Passou = {inverterGiro}, Sucesso = {Sucesso}, Entrada: {_ref}");

                        if (Sucesso)
                        {
                            await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoParada(Addr));
                            await Task.Delay(1500);

                            int _centro = Convert.ToInt32((_anguloEntrada + _anguloSaida) / 2.0);

                            await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoMovimento(Addr, _centro, 36, Sentido.Horario));
                            await Task.Delay(2500);
                            await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoSetZero(Addr));
                            await Task.Delay(1000);
                            break;
                        }

                        await Task.Delay(200); // Verificações rápidas.

                        VelocidadeGiro = DefineVelocidadeReferenciamento(EstadoInicial, anguloMaxVisitado, anguloMinVisitado);

                        if (
                            (_anguloAnterior == Angulo && RPM == 0) || // Verifica se o angulo nao foi alterado desde a ultima leitura, e o motor nao esta em movimento
                            (_velocidadeAnterior != VelocidadeGiro) // Verifica se mudou a velocidade de giro do motor de acordo com o angulo atual
                        )
                        {
                            await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoMovimento(Addr, _angulo, VelocidadeGiro, _sentido));
                        }

                        _anguloAnterior = Angulo;
                        _velocidadeAnterior = VelocidadeGiro;
                        if (Angulo > anguloMaxVisitado)
                        {
                            anguloMaxVisitado = Angulo;
                        }
                        else if (Angulo < anguloMinVisitado)
                        {
                            anguloMinVisitado = Angulo;
                        }
                    }

                    if (!Sucesso && inverterGiro)
                    {
                        await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoParada(Addr));
                        await Task.Delay(500);

                        _sentido = _sentido == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
                        _offset = Math.Min(100, _offset + 25); // Limita o offset máximo para evitar valores extremos.
                        int mx = _sentido == Sentido.Antihorario ? -1 : 1;
                        _angulo = (Math.Abs(_angulo) + _offset) * mx;
                        tentativas++;
                    }
                }
            }

            if (!Sucesso)
            {
                await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoParada(Addr));
            }

            MKS057DService.AddrRef = 0x00;
            MKS057DService.UltimoReferenciamento = DateTime.Now;
        }

        private int DefineVelocidadeReferenciamento(bool EstadoInicial, double anguloMaxVisitado, double anguloMinVisitado)
        {
            int VelocidadeGiro = 
                EstadoInicial || IN1_Atuado ? 10 : // Se o sensor estiver atuado ou o fluxo tiver iniciado com ele ja atuado, entao deve se mover lentamente para melhor precisao dos angulos de entrada e saida
                Angulo > (anguloMaxVisitado + 5) || Angulo < (anguloMinVisitado - 5) ? 18 : // Se o angulo atual ainda nao foi visitado, deve se mover em velocidade media
                20; // Se o angulo ja foi visitado, deve se mover em uma velocidade maior
            return VelocidadeGiro;
        }



    }

    public class MvdMotorMOVSensorimanetoGrafico
    {
        public DateTime Momento { get; set; }
        public string Modulo_ID { get; set; }
        public int RPM_SP { get; set; }
        public double RPM_Motor { get; set; }
        public double RPM_Roda { get; set; }
        public double Temperatura { get; set; }
        public double TemperaturaDriver { get; set; }
        public Sentido Sentido { get; set; }
        public double Velocidade { get; set; }
        public double Tensao { get; set; }
        public double Corrente { get; set; }
    }

    public class MvdServoFreioModel
    {
        public bool Incremental { get; set; }
        public int AnguloInicial { get; set; }
        private int AnguloControle { get; set; } = -1;
        private int AnguloControleMin
        {
            get
            {
                if (Incremental)
                {
                    return AnguloMin;
                }
                else
                {
                    return AnguloMax;
                }
            }
        }
        private int AnguloControleMax { get; set; } = 20;
        public bool Leitura { get; set; }
        private bool _controle { get; set; }
        public bool Controle
        {
            get
            {
                return _controle;
            }
        }
        private int RPM_SP { get; set; }
        private int MargemRPM { get; set; } = 2; // Margem de tolerância no RPM
        private int AnguloMin { get; set; } = 0; // Ângulo mínimo permitido pelo servo
        private int AnguloMax { get; set; } = 180; // Ângulo máximo permitido pelo servo
        private int FatorEscala { get; set; } = 10; // Fator de ajuste do controle proporcional

        public (bool, int) AtualizarDadosFreio(bool status, int rpm_sp = 0, int anguloInicial = -1, int anguloSP = -1)
        {
            bool mudou = _controle != status;
            _controle = status;
            RPM_SP = rpm_sp;

            if (!_controle)
            {
                AnguloControle = AnguloControleMin; // Solta o freio se não estiver no modo de controle
                return (mudou, AnguloControle);
            }

            if (anguloInicial == -1)
            {
                anguloInicial = AnguloInicial;
            }

            if (AnguloControle == -1)
            {
                AnguloControle = AnguloControleMin;
            }

            if (AnguloControle == AnguloControleMin)
            {
                AnguloControle = anguloInicial;
            }

            int ajuste = anguloSP;
            if (anguloSP == -1)
            {
                double Rpm_Atual = FuncoesMatematicas.CalculaRPMVelocidade(Variaveis.OperacaoEmAndamento.Sensoriamento.VelocidadeMedia);

                // Garante que os valores de RPM sejam válidos
                Rpm_Atual = Math.Max(Rpm_Atual, 0);
                RPM_SP = Math.Max(RPM_SP, 0);

                // Verifica se o RPM atual está dentro da margem de tolerância
                if (Math.Abs(Rpm_Atual - RPM_SP) <= MargemRPM)
                {
                    return (false, AnguloControle); // Se dentro da margem, não ajusta o ângulo
                }

                // Calcula o erro e ajusta proporcionalmente
                int erroRPM = RPM_SP - (int)Rpm_Atual;
                ajuste = erroRPM / FatorEscala;

                // Se incremental, aplica um ajuste limitado
                if (Incremental)
                {
                    ajuste = Math.Abs(ajuste); // Garante que o ajuste seja positivo
                }
                else
                {
                    ajuste = -Math.Abs(ajuste); // Garante que o ajuste seja negativo
                }
            }

            // Atualiza o ângulo de controle, garantindo que fique dentro dos limites
            int limiteMax = AnguloInicial + (Incremental ? AnguloControleMax : -AnguloControleMax);
            if (Incremental)
            {
                AnguloControle = (int)FuncoesMatematicas.Clamp(AnguloControle + ajuste, AnguloMin, limiteMax);
            }
            else
            {
                AnguloControle = (int)FuncoesMatematicas.Clamp(AnguloControle - ajuste, limiteMax, AnguloMax);
            }

            return (true, AnguloControle);
        }
    }



}