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 DirecionalModel
    {
        public string ID { get; set; } = T_Code.Dir.ToString();

        // SETs
        public byte _EnderecoCAN_Tx { get; set; }
        public byte _EnderecoCAN_Rx { get; set; }
        public string Mod_ID
        {
            get
            {
                return Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.DirMotor._EnderecoCAN_Tx == _EnderecoCAN_Tx)?.Modulo_ID ?? "";
            }
        }
        public Sentido Sentido_SP { get; set; } = Sentido.Parado;
        public double Angulo_SP { get; set; } = 0;
        public double FolgaMecanica { 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
                    Angulo > 0 ? Sentido.Horario :
                    Angulo < 0 ? Sentido.Antihorario :
                    Sentido.Parado;
            }
        }
        public Sentido SentidoReal
        {
            get
            {
                if (Mod_ID == "EF" || Mod_ID == "DT")
                {
                    return IN1_Atuado ? Sentido.Antihorario : Sentido.Horario;
                }
                else
                {
                    return IN1_Atuado ? Sentido.Horario : Sentido.Antihorario;
                }
            }
        }
        public double Angulo
        {
            get
            {
                return Leitura?.AnguloPulsos ?? 0;
            }
        }
        public double RPM
        {
            get
            {
                return Leitura?.RPM?.valor ?? 0;
            }
        }
        public bool IN1_Atuado
        {
            get
            {
                return !(Leitura?.Entradas?[0] ?? false);
            }
        }
        public bool IN2_Atuado
        {
            get
            {
                return !(Leitura?.Entradas?[1] ?? false);
            }
        }
        public bool Referenciando { get; set; } = false;
        public DateTime UltimoReferenciamento { get; set; }
        public bool Referenciado { get; set; } = false;

        public bool Comandar { get; set; } = false;
        public bool Inicializado
        {
            get
            {
                return Leitura?.Iniciado ?? false;
            }
        }
        private MKS057DModel _leitura = null;
        public MKS057DModel Leitura
        {
            get
            {
                _leitura = MKS057DCanService.DadosLeitura?.FirstOrDefault(x => x.EnderecoTx == _EnderecoCAN_Tx);
                return _leitura;
            }
            set
            {
                _leitura = value;
            }
        }

        public Direcao UltimaDirecao { get; set; } = Direcao.Parado;
        public DateTime UltimoComandoEnviado { get; set; } = DateTime.MinValue;
        public bool RetornandoAzero
        {
            get
            {
                bool foraZero = !FuncoesMatematicas.ValorEstaEntre(Angulo, 0, 1);
                bool parar = UltimaDirecao == Direcao.Parado;
                return foraZero && parar;
            }
        }
        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 DirecionalModel CarregarParametrosIniciais(string Mod_ID, byte _EnderecoTx, byte _EnderecoRx)
        {
            return new DirecionalModel()
            {
                ID = T_Code.Dir.ToString(),
                _EnderecoCAN_Tx = _EnderecoTx,
                _EnderecoCAN_Rx = _EnderecoRx,
                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)
        {
            var _Simulando = Variaveis.OperacaoEmAndamento.Simulando;

            if ((Inicializado && (Comandar || !AtingiuAnguloSP)) || _Simulando)
            {
                var _Controle = Variaveis.OperacaoEmAndamento.Controle;
                int AnguloTipoMovimento = VariaveisEquipamento.AnguloMovimento[_Controle.TipoMovimento];

                if (!Comandar)
                {
                    Sentido_SP = Sentido.Parado;
                }
                else if (Direcao == Direcao.Direita)
                {
                    Sentido_SP = ConfigSentidos[_Controle.TipoMovimento].Direita;
                }
                else if (Direcao == Direcao.Esquerda)
                {
                    Sentido_SP = ConfigSentidos[_Controle.TipoMovimento].Esquerda;
                }
                else
                {
                    Sentido_SP = Sentido.Parado;
                }

                UltimaDirecao = Direcao;

                if (Sentido_SP == Sentido.Parado)
                {
                    Angulo_SP = 0;
                    UltimaDirecao = Direcao.Parado;
                }
                else if (AnguloTipoMovimento != -1)
                {
                    Angulo_SP = AnguloTipoMovimento;
                }
                else
                {
                    Angulo_SP = _Controle.Angulo + (FolgaMecanica / 2.0);
                }

                if (Comandar)
                {
                    Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle = Variaveis.OperacaoEmAndamento.Controle.Angulo;
                    //Console.WriteLine($"[{Mod_ID}] Angulo Atualizado para {Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle}");
                }

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

                Angulo_SP = Math.Abs(Angulo_SP) * mx;

                if (_Simulando)
                {
                    return;
                }

                //MKS057DService.AdicionarComandoNaFila(MKS057DService.MontarComandoMovimento(EnderecoByte, Angulo_SP, Velocidade, Sentido_SP));
                MKS057DCanService.Movimento(_EnderecoCAN_Tx, _EnderecoCAN_Rx, Angulo_SP, Velocidade, Sentido_SP);
                UltimoComandoEnviado = DateTime.Now;
            }

        }

        public async Task ReferenciaMotor()
        {
            Referenciando = true;

            await Task.Delay(1000);

            Referenciado = false;

            await SetZero();

            bool Sucesso = false;
            bool EstadoInicial = IN1_Atuado;
            double AnguloEntrada = 9999;
            double AnguloSaida = 9999;
            int verificacoesAngulo = 0;
            double anguloAnterior = 0;

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

            Sentido _sentido = SentidoReal == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
            double _anguloSP = 360 * (_sentido == Sentido.Antihorario ? -1 : 1);
            double _anguloSpAnterior = 0;

            while (!Sucesso && TimeoutGeral > DateTime.Now && Inicializado)
            {
                int VelocidadeGiro = DefineVelocidadeReferenciamento();

                if (_anguloSP != _anguloSpAnterior || (verificacoesAngulo > 5 && Angulo == anguloAnterior))
                {
                    MKS057DCanService.Movimento(_EnderecoCAN_Tx, _EnderecoCAN_Rx, _anguloSP, VelocidadeGiro, _sentido);
                    _anguloSpAnterior = _anguloSP;
                    verificacoesAngulo = 0;
                }

                if (IN1_Atuado != EstadoInicial && AnguloEntrada == 9999)
                {
                    AnguloEntrada = Leitura.AnguloEstimado();
                    Console.WriteLine($"[DIR {Mod_ID}] - Angulo de Entrada Definido: {AnguloEntrada} (Leitura: {Angulo})");
                    
                    _sentido = _sentido == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
                    _anguloSP *= -1;
                    Console.WriteLine($"[DIR {Mod_ID}] - Invertendo Sentido de Giro para {_sentido.ToString()}");
                }
                if (IN1_Atuado == EstadoInicial && AnguloEntrada != 9999 && AnguloSaida == 9999)
                {
                    AnguloSaida = Leitura.AnguloEstimado();
                    Console.WriteLine($"[DIR {Mod_ID}] - Angulo de Saída Definido: {AnguloSaida} (Leitura: {Angulo})");

                    _sentido = _sentido == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
                    Console.WriteLine($"[DIR {Mod_ID}] - Invertendo Sentido de Giro para {_sentido.ToString()}");

                    _anguloSP = CalcularCentroCompensado(AnguloEntrada, AnguloSaida, _sentido);
                }

                verificacoesAngulo++;
                anguloAnterior = Angulo;

                Sucesso = AnguloEntrada != 9999 && AnguloSaida != 9999 && (FuncoesMatematicas.ValorEstaEntre(Leitura.AnguloEstimado(), _anguloSP, 0.5));
                if (Sucesso)
                {
                    await SetZero();
                    Console.WriteLine($"[DIR {Mod_ID}] - Referenciamento Concluído!");
                    Referenciado = true;
                }

                DateTime Inicio = DateTime.Now;
                bool NovoDado() => MKS057DCanService.DadosLeitura.Any(x => x.EnderecoRx == _EnderecoCAN_Rx && x.UltimoComandoRecebido > Inicio);
                await FuncoesGlobais.AguardarCondicaoAsync(NovoDado, 500, 100);
            }

            Referenciando = false;

            UltimoReferenciamento = DateTime.Now;
        }

        private int DefineVelocidadeReferenciamento()
        {
            int VelocidadeGiro = 10;
            return VelocidadeGiro;
        }

        private double CalcularCentroCompensado(double entrada, double saida, Sentido sentido)
        {
            double centro = (entrada + saida) / 2.0;
            double folga = Math.Abs(entrada - saida);

            // Determina o sentido do giro baseado na diferença
            double direcao = sentido == Sentido.Horario ? -1 : 1;

            double compensacao = 0; // (folga / 2.0) * direcao;

            double centroCompensado = centro + compensacao;

            Console.WriteLine($"[DIR {Mod_ID}] - Folga: {folga}, Entrada: {entrada}, Saida: {saida}, Centro: {centro}, Compensacao: {compensacao}, Centro Compensado: {centroCompensado}");

            FolgaMecanica = folga;

            return centroCompensado;
        }

        public async Task SetZero()
        {
            MKS057DCanService.SetZero(_EnderecoCAN_Tx, _EnderecoCAN_Rx);

            for (int i = 0; i < 5; i++)
            {
                DateTime Inicio = DateTime.Now;
                bool NovoDado() => MKS057DCanService.DadosLeitura.Any(x => x.EnderecoRx == _EnderecoCAN_Rx && x.UltimoComandoRecebido > Inicio);
                await FuncoesGlobais.AguardarCondicaoAsync(NovoDado, 500, 100);

                if (FuncoesMatematicas.ValorEstaEntre(Angulo, -1, 1))
                {
                    Console.WriteLine($"[DIR {Mod_ID}] - Set Zero");
                    break;
                }
                else
                {
                    MKS057DCanService.SetZero(_EnderecoCAN_Tx, _EnderecoCAN_Rx);
                }
            }

            MKS057DCanService.Movimento(_EnderecoCAN_Tx, _EnderecoCAN_Rx, 0, 10, Sentido.Parado);
            Console.WriteLine($"[DIR {Mod_ID}] - Comando Angulo Zero: {Angulo}");
        }

    }
}