using System.Collections.Generic;
using System.Linq;
using System.Windows.Forms;
using System;
using static AgroBase.Models.Enums;
using AgroBase.Forms;
using AgroBase.Models;
using System.Threading.Tasks;
using AgroBase.Models.Modules;
using SharpDX.DirectInput;
using AgroBase.Services;
using System.Data;

namespace AgroBase
{
    public class GeneralJoystick
    {
        public static Joystick JoystickConectado = null;
        public static int MargemAnalogico = 100;
        public static int MaximoAnalogico = 65534;

        public static List<ComandosDispositivo> Comandos = new List<ComandosDispositivo>() { };
        public static List<DeParaComandos> DeParaComandos = new List<DeParaComandos>()
        {
            new DeParaComandos() { key = Keys.Left,     Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.SEsquerda,  PararAoSoltar = true },
            new DeParaComandos() { key = Keys.Left,     Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.AEEsquerda, PararAoSoltar = true },
            new DeParaComandos() { key = Keys.Right,    Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.SDireita,   PararAoSoltar = true },
            new DeParaComandos() { key = Keys.Right,    Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.AEDireita,  PararAoSoltar = true },
            new DeParaComandos() { key = Keys.R,        Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.R3,         PararAoSoltar = false },
            new DeParaComandos() { key = Keys.Up,       Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Xis,        PararAoSoltar = true },
            new DeParaComandos() { key = Keys.Up,       Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.R2A,        PararAoSoltar = true },
            new DeParaComandos() { key = Keys.Down,     Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Quadrado,   PararAoSoltar = true },
            new DeParaComandos() { key = Keys.Down,     Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.L2A,        PararAoSoltar = true },
            new DeParaComandos() { key = Keys.Space,    Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Bolinha,    PararAoSoltar = true },
            new DeParaComandos() { key = Keys.Subtract, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.L1,         PararAoSoltar = false },
            new DeParaComandos() { key = Keys.Add,      Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.R1,         PararAoSoltar = false },
            new DeParaComandos() { key = Keys.D1,       Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADEsquerda, PararAoSoltar = true, IDcontrolar = "B01" },
            new DeParaComandos() { key = Keys.D2,       Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADBaixo,    PararAoSoltar = true, IDcontrolar = "B02" },
            new DeParaComandos() { key = Keys.D3,       Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADCima,     PararAoSoltar = true, IDcontrolar = "B03" },
            new DeParaComandos() { key = Keys.D4,       Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADDireita,  PararAoSoltar = true, IDcontrolar = "B04" },
        };
        public static List<BotoesJoystick> BotoesJoyPressionados = new List<BotoesJoystick>() { };
        public static List<TipoMovimentoDirecional> MovimentosCombinados = new List<TipoMovimentoDirecional>()
        {
            TipoMovimentoDirecional.MovimentoLateral, 
            TipoMovimentoDirecional.RotacionarNoEixo
        };

        public static Direcao UltimaDirecao = Direcao.Parado;
        public static bool Pressionado = false;
        public static List<IDispositivosService> DispCon;

        //public static int RPM { get; set; } = 60;
        //public static double Angulo { get; set; } = 20;
        //public static int Velocidade { get; set; } = 50;

        public static AsyncTaskTimerModel tmrJoystick;

        public static void IniciarRotinas()
        {
            tmrJoystick = new AsyncTaskTimerModel("tmrJoystick", tmrJoystick_Tick, 100);
        }

        public static void AtualizaDispositivo()
        {
            Func<bool> funcAtt = new Func<bool>(() =>
            {
                DispCon = Variaveis.DispositivosConectados.Where(x => x.Dados.ConexaoAtiva).ToList();
                frmInstancial.frmJoystick.lblDispositivo.Text = "Dispositivos: " + (DispCon.Count == 0 ? "N/A" : string.Join(", ", DispCon.Select(x => x._Descricao + " (" + Enum.GetName(typeof(T_Code), x.Dispositivo) + ")").ToArray()));
                frmInstancial.frmPrincipal.cmbJoysticks.Items.Clear();

                InicializarJoysticksConectados();

                return true;
            });
            FuncoesGlobais.ExecutarMetodoComVerificacaoCrossThread(frmInstancial.frmPrincipal.cmbJoysticks, funcAtt);
        }

        public static void InicializarJoysticksConectados()
        {
            var directInput = new DirectInput();

            // Find a Joystick Guid
            var joystickGuid = Guid.Empty;

            foreach (var deviceInstance in directInput.GetDevices(DeviceType.Gamepad, DeviceEnumerationFlags.AllDevices))
            {
                joystickGuid = deviceInstance.InstanceGuid;
                frmInstancial.frmPrincipal.cmbJoysticks.Items.Add(deviceInstance.InstanceName);
            }

            // If Gamepad not found, look for a Joystick
            if (joystickGuid == Guid.Empty)
            {
                foreach (var deviceInstance in directInput.GetDevices(DeviceType.Joystick, DeviceEnumerationFlags.AllDevices))
                {
                    joystickGuid = deviceInstance.InstanceGuid;
                    frmInstancial.frmPrincipal.cmbJoysticks.Items.Add(deviceInstance.InstanceName);
                }
            }

            // If Joystick not found, throws an error
            if (joystickGuid == Guid.Empty)
            {
                JoystickConectado = null;
                tmrJoystick?.Stop();

                return;
            }

            // Instantiate the joystick
            JoystickConectado = new Joystick(directInput, joystickGuid);

            // Query all suported ForceFeedback effects
            var allEffects = JoystickConectado.GetEffects();
            foreach (var effectInfo in allEffects)
            {
                Console.WriteLine("Effect available {0}", effectInfo.Name);
            }

            // Set BufferSize in order to use buffered data.
            JoystickConectado.Properties.BufferSize = 128;

            // Acquire the joystick
            JoystickConectado.Acquire();


            if (JoystickConectado != null)
            {
                tmrJoystick.Start();
                frmInstancial.frmPrincipal.cmbJoysticks.SelectedIndex = 0;
            }

        }

        private static bool ComparaPosicaoAnalogico(int Atual, ComparativoAnalogico Comparativo)
        {
            int Meio = MaximoAnalogico / 2;
            bool Pressionado = false;

            if (Comparativo == ComparativoAnalogico.Igual)
            {
                Pressionado = Atual >= (Meio - MargemAnalogico) && Atual <= (Meio + MargemAnalogico);
            }
            else if (Comparativo == ComparativoAnalogico.Maior)
            {
                Pressionado = Atual >= (Meio + MargemAnalogico);
            }
            else if (Comparativo == ComparativoAnalogico.Menor)
            {
                Pressionado = Atual <= (Meio - MargemAnalogico);
            }

            return Pressionado;
        }

        private static double DeParaAnanlogico(double Valor, double DeMin, double DeMax, double ParaMin, double ParaMax)
        {
            if (Valor < DeMin)
            {
                Valor = DeMin;
            }
            else if (Valor > DeMax)
            {
                Valor = DeMax;
            }

            double scale = (Valor - DeMin) / (DeMax - DeMin);
            double mappedValue = scale * (ParaMax - ParaMin) + ParaMin;

            return mappedValue;
        }

        private static async Task tmrJoystick_Tick()
        {
            if (JoystickConectado == null)
            {
                return;
            }
            JoystickUpdate[] datas;
            try
            {
                JoystickConectado.Poll();
                datas = JoystickConectado.GetBufferedData();
            }
            catch
            {
                EnviaComandoMotor(Keys.Escape, T_Code.Mov, ForcarComando: true);
                EnviaComandoMotor(Keys.Escape, T_Code.Dir, ForcarComando: true);
                AtualizaDispositivo();
                return;
            }
            

            List<BotoesPressionadosValores> _botoesPressionados = new List<BotoesPressionadosValores>();
            List<BotoesJoystick> _botoesSoltos = new List<BotoesJoystick>();

            foreach (var state in datas)
            {
                //Console.WriteLine(state);
                if (state.Offset == JoystickOffset.PointOfViewControllers0)
                {
                    if (state.Value == -1)
                    {
                        if (BotoesJoyPressionados.Any(x => x == BotoesJoystick.SBaixo))
                        {
                            _botoesSoltos.Add(BotoesJoystick.SBaixo);
                        }
                        else if (BotoesJoyPressionados.Any(x => x == BotoesJoystick.SCima))
                        {
                            _botoesSoltos.Add(BotoesJoystick.SCima);
                        }
                        else if (BotoesJoyPressionados.Any(x => x == BotoesJoystick.SEsquerda))
                        {
                            _botoesSoltos.Add(BotoesJoystick.SEsquerda);
                        }
                        else if (BotoesJoyPressionados.Any(x => x == BotoesJoystick.SDireita))
                        {
                            _botoesSoltos.Add(BotoesJoystick.SDireita);
                        }
                    }
                    else if (state.Value == 0)
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.SCima, valor = state.Value });
                    }
                    else if (state.Value == 9000)
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.SDireita, valor = state.Value });
                    }
                    else if (state.Value == 18000)
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.SBaixo, valor = state.Value });
                    }
                    else if (state.Value == 27000)
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.SEsquerda, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.Z)
                {
                    if (ComparaPosicaoAnalogico(state.Value, ComparativoAnalogico.Igual))
                    {
                        if (BotoesJoyPressionados.Any(x => x == BotoesJoystick.ADDireita))
                        {
                            _botoesSoltos.Add(BotoesJoystick.ADDireita);
                        }
                        if (BotoesJoyPressionados.Any(x => x == BotoesJoystick.ADEsquerda))
                        {
                            _botoesSoltos.Add(BotoesJoystick.ADEsquerda);
                        }
                    }
                    else if (ComparaPosicaoAnalogico(state.Value, ComparativoAnalogico.Maior))
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.ADDireita, valor = state.Value });
                    }
                    else if (ComparaPosicaoAnalogico(state.Value, ComparativoAnalogico.Menor))
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.ADEsquerda, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.X)
                {
                    if (ComparaPosicaoAnalogico(state.Value, ComparativoAnalogico.Igual))
                    {
                        if (BotoesJoyPressionados.Any(x => x == BotoesJoystick.AEEsquerda))
                        {
                            _botoesSoltos.Add(BotoesJoystick.AEEsquerda);
                        }
                        if (BotoesJoyPressionados.Any(x => x == BotoesJoystick.AEDireita))
                        {
                            _botoesSoltos.Add(BotoesJoystick.AEDireita);
                        }
                    }
                    else if (ComparaPosicaoAnalogico(state.Value, ComparativoAnalogico.Maior))
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.AEDireita, valor = state.Value });
                    }
                    else if (ComparaPosicaoAnalogico(state.Value, ComparativoAnalogico.Menor))
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.AEEsquerda, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.Y)
                {
                    if (ComparaPosicaoAnalogico(state.Value, ComparativoAnalogico.Igual))
                    {
                        if (BotoesJoyPressionados.Any(x => x == BotoesJoystick.AECima))
                        {
                            _botoesSoltos.Add(BotoesJoystick.AECima);
                        }
                        if (BotoesJoyPressionados.Any(x => x == BotoesJoystick.AEBaixo))
                        {
                            _botoesSoltos.Add(BotoesJoystick.AEBaixo);
                        }
                    }
                    else if (ComparaPosicaoAnalogico(state.Value, ComparativoAnalogico.Maior))
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.AEBaixo, valor = state.Value });
                    }
                    else if (ComparaPosicaoAnalogico(state.Value, ComparativoAnalogico.Menor))
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.AECima, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.RotationX)
                {
                    if (state.Value == 0)
                    {
                        if (BotoesJoyPressionados.Any(x => x == BotoesJoystick.L2A))
                        {
                            _botoesSoltos.Add(BotoesJoystick.L2A);
                        }
                    }
                    else if (state.Value > 0)
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.L2A, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.RotationY)
                {
                    if (state.Value == 0)
                    {
                        if (BotoesJoyPressionados.Any(x => x == BotoesJoystick.R2A))
                        {
                            _botoesSoltos.Add(BotoesJoystick.R2A);
                        }
                    }
                    else if (state.Value > 0)
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.R2A, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.RotationZ)
                {
                    if (ComparaPosicaoAnalogico(state.Value, ComparativoAnalogico.Igual))
                    {
                        if (BotoesJoyPressionados.Any(x => x == BotoesJoystick.ADCima))
                        {
                            _botoesSoltos.Add(BotoesJoystick.ADCima);
                        }
                        if (BotoesJoyPressionados.Any(x => x == BotoesJoystick.ADBaixo))
                        {
                            _botoesSoltos.Add(BotoesJoystick.ADBaixo);
                        }
                    }
                    else if (ComparaPosicaoAnalogico(state.Value, ComparativoAnalogico.Maior))
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.ADBaixo, valor = state.Value });
                    }
                    else if (ComparaPosicaoAnalogico(state.Value, ComparativoAnalogico.Menor))
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.ADCima, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.Buttons0)
                {
                    if (state.Value == 0)
                    {
                        _botoesSoltos.Add(BotoesJoystick.Quadrado);
                    }
                    else
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.Quadrado, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.Buttons1)
                {
                    if (state.Value == 0)
                    {
                        _botoesSoltos.Add(BotoesJoystick.Xis);
                    }
                    else
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.Xis, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.Buttons2)
                {
                    if (state.Value == 0)
                    {
                        _botoesSoltos.Add(BotoesJoystick.Bolinha);
                    }
                    else
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.Bolinha, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.Buttons3)
                {
                    if (state.Value == 0)
                    {
                        _botoesSoltos.Add(BotoesJoystick.Triangulo);
                    }
                    else
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.Triangulo, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.Buttons4)
                {
                    if (state.Value == 0)
                    {
                        _botoesSoltos.Add(BotoesJoystick.L1);
                    }
                    else
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.L1, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.Buttons5)
                {
                    if (state.Value == 0)
                    {
                        _botoesSoltos.Add(BotoesJoystick.R1);
                    }
                    else
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.R1, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.Buttons6)
                {
                    if (state.Value == 0)
                    {
                        _botoesSoltos.Add(BotoesJoystick.L2);
                    }
                    else
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.L2, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.Buttons7)
                {
                    if (state.Value == 0)
                    {
                        _botoesSoltos.Add(BotoesJoystick.R2);
                    }
                    else
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.R2, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.Buttons8)
                {
                    if (state.Value == 0)
                    {
                        _botoesSoltos.Add(BotoesJoystick.Share);
                    }
                    else
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.Share, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.Buttons9)
                {
                    if (state.Value == 0)
                    {
                        _botoesSoltos.Add(BotoesJoystick.Options);
                    }
                    else
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.Options, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.Buttons10)
                {
                    if (state.Value == 0)
                    {
                        _botoesSoltos.Add(BotoesJoystick.L3);
                    }
                    else
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.L3, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.Buttons11)
                {
                    if (state.Value == 0)
                    {
                        _botoesSoltos.Add(BotoesJoystick.R3);
                    }
                    else
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.R3, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.Buttons12)
                {
                    if (state.Value == 0)
                    {
                        _botoesSoltos.Add(BotoesJoystick.PS);
                    }
                    else
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.PS, valor = state.Value });
                    }
                }
                else if (state.Offset == JoystickOffset.Buttons13)
                {
                    if (state.Value == 0)
                    {
                        _botoesSoltos.Add(BotoesJoystick.Touchpad);
                    }
                    else
                    {
                        _botoesPressionados.Add(new BotoesPressionadosValores() { botao = BotoesJoystick.Touchpad, valor = state.Value });
                    }
                }
            }

            var botoes_pressionados_g = _botoesPressionados
                .GroupBy(x => x.botao)
                .Select(x => new {
                    botao = x.Key,
                    dados = x
                })
                .ToList();
            botoes_pressionados_g.ForEach(botao =>
            {
                EnviarComandoMotorAnalogico(botao.botao, botao.dados?.LastOrDefault()?.valor ?? 0, false);
                //Console.WriteLine($"Botao pressionado: {botao.botao}, Valor: {string.Join(", ", botao.dados.Select(y => y.valor))}");
                BotoesJoyPressionados.Add(botao.botao);
                var Comando = DeParaComandos.FirstOrDefault(x => x.botaoJoy == botao.botao);
                if (Comando != null)
                {
                    EnviaComandoMotor(Comando.key, Comando.Dispositivo, ID: Comando.IDcontrolar, botao: botao.botao);
                }
            });
            _botoesSoltos.ForEach(botao =>
            {
                EnviarComandoMotorAnalogico(botao, 0, true);
                //Console.WriteLine($"Botao solto: {botao}");
                BotoesJoyPressionados.Remove(botao);
                var Comando = DeParaComandos.FirstOrDefault(x => x.botaoJoy == botao);
                if (Comando != null && Comando.PararAoSoltar)
                {
                    EnviaComandoMotor(Keys.Escape, Comando.Dispositivo, ID: Comando.IDcontrolar, botao: botao, ForcarComando: true);
                }
            });
        }

        private static void EnviarComandoMotorAnalogico(BotoesJoystick botao, double valor, bool solto)
        {
            T_Code dispositivo = T_Code.Vzo;
            Direcao direcao = Direcao.Parado;
            double percent = 0;
            double centro = 32767;
            double deadzone = 3000;
            double maximo = 65535.0;
            var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;

            switch (botao)
            {
                case BotoesJoystick.AEEsquerda:
                    dispositivo = T_Code.Dir;
                    direcao = Direcao.Esquerda;
                    if (valor < centro - deadzone)
                        percent = (centro - valor) / centro;
                    else
                        percent = 0;
                    break;

                case BotoesJoystick.AEDireita:
                    direcao = Direcao.Direita;
                    dispositivo = T_Code.Dir;
                    if (valor > centro + deadzone)
                        percent = (valor - centro) / (maximo - centro);
                    else
                        percent = 0;
                    break;

                case BotoesJoystick.AECima:
                    if (valor < centro - deadzone)
                        percent = (centro - valor) / centro;
                    else
                        percent = 0;
                    break;

                case BotoesJoystick.AEBaixo:
                    if (valor > centro + deadzone)
                        percent = (valor - centro) / (maximo - centro);
                    else
                        percent = 0;
                    break;

                case BotoesJoystick.SEsquerda:
                    direcao = Direcao.Esquerda;
                    dispositivo = T_Code.Dir;
                    percent = 1.0;
                    break;

                case BotoesJoystick.SDireita:
                    direcao = Direcao.Direita;
                    dispositivo = T_Code.Dir;
                    percent = 1.0;
                    break;

                case BotoesJoystick.L2A:
                    direcao = Direcao.Tras;
                    dispositivo = T_Code.Mov;
                    percent = valor / maximo;
                    break;

                case BotoesJoystick.R2A:
                    direcao = Direcao.Frente;
                    dispositivo = T_Code.Mov;
                    percent = valor / maximo;
                    break;

                case BotoesJoystick.Xis:
                    direcao = Direcao.Frente;
                    dispositivo = T_Code.Mov;
                    percent = ((double)pControle.MovVelocidadeCErvasPercent / (double)pControle.MovVelocidadeSErvasPercent);
                    break;

                case BotoesJoystick.Quadrado:
                    direcao = Direcao.Tras;
                    dispositivo = T_Code.Mov;
                    percent = ((double)pControle.MovVelocidadeCErvasPercent / (double)pControle.MovVelocidadeSErvasPercent);
                    break;

            }

            percent = FuncoesMatematicas.Clamp(percent, 0, 1);

            //Console.WriteLine($"botao: {botao}, valor: {valor}, percentual: {percent * 100.0:F2}%");

            var _Controle = Variaveis.OperacaoEmAndamento.Controle;
            _Controle.Direcao = direcao;
            int mx = direcao == Direcao.Esquerda || direcao == Direcao.Tras ? -1 : 1;
            switch (dispositivo)
            {
                case T_Code.Dir:
                    _Controle.Angulo = solto ? 0 : pControle.DirAnguloMaximo * percent * mx;
                    break;
                case T_Code.Mov:
                    double map = FuncoesMatematicas.Map(valor, 0, maximo, pControle.MovVelocidadeCErvasPercent, pControle.MovVelocidadeSErvasPercent);
                    _Controle.PercentualVelocidadeSP = solto ? 0 : map * mx;
                    break;
            }
        }

        public static void EnviaComandoMotor(Keys Tecla, T_Code Disp = T_Code.Vzo, string ID = null, BotoesJoystick botao = BotoesJoystick.Vazio, bool ForcarComando = false)
        {
            // Verifica se deve interromper o fluxo
            // interrompe se nao estiver diagnosticando, nao forca o envio, e se o modo for manual vai verificar o status da operacao e do sonar, pois automatico faz esse controle em outro lugar
            bool deveInterromper =
                !Forms.IHM.frmDiagnosticos.Diagnosticando && !ForcarComando &&
                (
                    Variaveis.OperacaoEmAndamento.Modo == ModoOperacao.Manual && (
                        Variaveis.OperacaoEmAndamento.Calibrando ||
                        !Variaveis.OperacaoEmAndamento.OperacaoLiberada ||
                        !Variaveis.OperacaoEmAndamento.Iniciado ||
                        !VerificaComandoValidoSonar(Tecla)
                    )
                );

            if (deveInterromper && false)
            {
                return;
            }

            ComandosDispositivo Comando = new ComandosDispositivo()
            {
                Momento = DateTime.Now,
                Comando = Tecla, 
                IDcontrolar = DeParaComandos.FirstOrDefault(x => x.key == Tecla)?.IDcontrolar ?? "",
            };

            Direcao DirecaoAtual = Direcao.Parado;

            var _Controle = Variaveis.OperacaoEmAndamento.Controle;
            var tipoControle = _Controle?.TiposControle?.FirstOrDefault(x => x?.Tipo == Comando.Dispositivo);

            // Cancelar Comando Anterior
            if (Comando.Comando == Keys.Escape)
            {
                var ComandosDisp = Disp == T_Code.Vzo ? Comandos : Comandos.Where(x => x.Dispositivo == Disp).ToList();
                
                if (ComandosDisp.Any())
                {
                    if (Disp == T_Code.Vzo)
                    {
                        Comando.Dispositivo = ComandosDisp[ComandosDisp.Count - 1].Dispositivo;
                        Comando.IDcontrolar = ComandosDisp[ComandosDisp.Count - 1].IDcontrolar;
                    }
                    else
                    {
                        Comando.Dispositivo = Disp;
                        Comando.IDcontrolar = !string.IsNullOrEmpty(ID) ? ID : ComandosDisp[ComandosDisp.Count - 1].IDcontrolar;
                    }
                    DirecaoAtual = Direcao.Parado;

                    switch (botao)
                    {
                        case BotoesJoystick.Bolinha:
                            _Controle.EmFreio = false;
                            break;
                    }
                    
                    Pressionado = true;
                }
            }
            // Controle de Velocidade (MOV RPM SP)
            else if (Comando.Comando == Keys.Add)
            {
                Comando.Dispositivo = T_Code.Mov;
                DirecaoAtual = tipoControle?.UltimaDirecao ?? Direcao.Parado;
                int s = Math.Sign(_Controle.PercentualVelocidadeSP);
                var p = (Math.Abs(_Controle.PercentualVelocidadeSP) + 1) * s;
                _Controle.PercentualVelocidadeSP = p;
                Pressionado = true;
            }
            else if (Comando.Comando == Keys.Subtract)
            {
                Comando.Dispositivo = T_Code.Mov;
                DirecaoAtual = tipoControle?.UltimaDirecao ?? Direcao.Parado;
                int s = Math.Sign(_Controle.PercentualVelocidadeSP);
                var p = (Math.Abs(_Controle.PercentualVelocidadeSP) - 1) * s;
                _Controle.PercentualVelocidadeSP = p;
                Pressionado = true;
            }
            // Controle Direcao dos Motores BLDC (MOV)
            else if (Comando.Comando == Keys.Up) // && UltimaDirecao != Direcao.Frente
            {
                Comando.Dispositivo = T_Code.Mov;
                DirecaoAtual = Direcao.Frente;
                Pressionado = true;
            }
            else if (Comando.Comando == Keys.Down) // && UltimaDirecao != Direcao.Tras
            {
                Comando.Dispositivo = T_Code.Mov;
                DirecaoAtual = Direcao.Tras;
                Pressionado = true;
            }
            else if (Comando.Comando == Keys.Space && UltimaDirecao != Direcao.EmFreio)
            {
                Comando.Dispositivo = T_Code.Mov;
                DirecaoAtual = Direcao.EmFreio;
                _Controle.EmFreio = true;
                Pressionado = true;
            }
            // Controle de Rotação dos Motores de Passo (DIR)
            else if (Comando.Comando == Keys.Left) // && UltimaDirecao != Direcao.Esquerda
            {
                Comando.Dispositivo = T_Code.Dir;
                DirecaoAtual = Direcao.Esquerda;
                Pressionado = true;
            }
            else if (Comando.Comando == Keys.Right) // && UltimaDirecao != Direcao.Direita
            {
                Comando.Dispositivo = T_Code.Dir;
                DirecaoAtual = Direcao.Direita;
                Pressionado = true;
            }
            else if (Comando.Comando == Keys.R)
            {
                Comando.Dispositivo = T_Code.Dir;
                DirecaoAtual = Direcao.Referenciar;
                Pressionado = true;
            }
            // Controle dos Bicos Pulverizadores (ATU)
            else if ((Comando.Comando == Keys.D1 || Comando.Comando == Keys.D2 || Comando.Comando == Keys.D3 || Comando.Comando == Keys.D4) && (Variaveis.OperacaoEmAndamento.DispAtu?.Dados?.BicosPulverizadores?.Any(x => x?.ID == Comando.IDcontrolar && !(x?.ComandoAtuar ?? false)) ?? false))
            {
                Comando.Dispositivo = T_Code.Atu;
                Pressionado = true;
            }
            // Soltar o Botão
            else
            {
                Pressionado = false;
            }

            if (Pressionado)
            {
                Comandos.Add(Comando);

                /*for (int i = 0; i < 1; i++)
                {
                    var Dispositivo = DispCon.FirstOrDefault(x => x.Dispositivo == Comando.Dispositivo);
                    if (Dispositivo != null)
                    {
                        var Protocolos =
                            Dispositivo.Dados is MovimentacaoModel ? ((MovimentacaoModel)Dispositivo.Dados).Motores.Select(x => x.ProtocoloComando(DirecaoAtual)) :
                            Dispositivo.Dados is DirecionalModel ? ((DirecionalModel)Dispositivo.Dados).Motores.Select(x => x.ProtocoloComando(DirecaoAtual)) :
                            null;
                        foreach (var ProtocoloMotor in Protocolos)
                        {
                            Dispositivo.EnviarDadosSerial(ProtocoloMotor);
                            Task.Delay(10);
                        }
                    }
                    
                    Task.Delay(100);
                }*/

                for (int i = 0; i < 1; i++)
                {
                    switch (Comando.Dispositivo)
                    {
                        case T_Code.Mov:
                            {
                                //Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.ForEach(x => x.MovMotor.EnviarComandoControle(DirecaoAtual));
                                Variaveis.OperacaoEmAndamento.AtualizarDadosControle(ForcarComando, new List<T_Code>() { Comando.Dispositivo });
                                break;
                            }
                        case T_Code.Dir:
                            {
                                List<string> modsControlar = null;
                                if (!string.IsNullOrEmpty(ID))
                                    modsControlar = new List<string>() { ID };
                                if (MovimentosCombinados.Contains(Variaveis.OperacaoEmAndamento.Controle.TipoMovimento))
                                {
                                    Task.Run(async () => await Variaveis.OperacaoEmAndamento.DispMvd.Dados.ExecutarMovimentoDirecionalCombinado(DirecaoAtual));
                                }
                                else if (DirecaoAtual == Direcao.Referenciar)
                                {
                                    Task.Run(async () => await Variaveis.OperacaoEmAndamento.DispMvd.Dados.RealizarReferenciamentoDirecional(ModsReferenciar: modsControlar));
                                }
                                else
                                {
                                    Variaveis.OperacaoEmAndamento.AtualizarDadosControle(ForcarComando, new List<T_Code>() { Comando.Dispositivo });
                                    //foreach (var mod in Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos)
                                    //{
                                    //    if (modsControlar == null || modsControlar.Contains(mod.DirMotor.Mod_ID))
                                    //    {
                                    //        mod.DirMotor.EnviarComandoControle(DirecaoAtual);
                                    //
                                    //        if (mod.MovMotor.CompensacaoNecessaria)
                                    //        {
                                    //            Direcao ultimaDirecaoMov = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov)?.UltimaDirecao ?? Direcao.Parado;
                                    //            mod.MovMotor.EnviarComandoControle(ultimaDirecaoMov);
                                    //        }
                                    //    }
                                    //};
                                }
                                break;
                            }
                        case T_Code.Atu:
                            {
                                EnviarComandoAtuador(S_Code.sBIC, Comando.IDcontrolar, Status: Comando.Comando == Keys.Escape ? false : true);
                                break;
                            }
                    }

                    //Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == Comando.Dispositivo);
                    //if (Controle != null)
                    //{
                    //    Controle.UltimaDirecao = DirecaoAtual;
                    //}
                }

                UltimaDirecao = DirecaoAtual;

                AtualizaConsole(Enum.GetName(typeof(Direcao), UltimaDirecao));
            }
        }

        public static void EnviarComandoAtuador(S_Code Componente, string ID, bool? Status = null, double? Valor = null, bool Forcar = false)
        {
            var DispAtu = Variaveis.OperacaoEmAndamento.DispAtu;
            if (!DispAtu.Dados.Conectado)
                return;

            if (!Forcar)
            {
                if (!Variaveis.OperacaoEmAndamento.Iniciado) return;
                bool componente_em_uso = Variaveis.OperacaoEmAndamento.Parametros.ModulosMandatorios.Any(x => x.Dispositivo == T_Code.Atu && (x.ComponentesEmUso?.ContainsKey(ID) ?? false) && x.ComponentesEmUso[ID].Item2);
                if (!componente_em_uso && (Status ?? false)) return; // Impede execucao de comandos que liguem componentes quando estiver marcado para nao utilizar
            }
            
            int id_num = -1;
            CanMessagePosicaoDados posicao = CanMessagePosicaoDados.Command1;
            byte[] data = null;

            switch (Componente)
            {
                case S_Code.sMOD:
                    if (DispAtu?.Dados != null && Status == true)
                    {
                        (id_num, posicao, data) = DispAtu.Dados.ProtocoloComandoReset();
                    }
                    break;
                case S_Code.sBMB:
                    var Bomba = DispAtu?.Dados?.BombasPressurizadoras?.FirstOrDefault(x => x.ID == ID);
                    if (Bomba != null)
                    {
                        if (Status != null) Bomba.ComandoAtuar = (bool)Status;
                        if (Valor != null) Variaveis.OperacaoEmAndamento.Parametros.Controle.AtuPressaoLinha = (double)Valor;
                        (id_num, posicao, data) = Bomba.ProtocoloComando();
                    }
                    break;
                case S_Code.sBIC:
                    var Bico = DispAtu.Dados.BicosPulverizadores.FirstOrDefault(x => x.ID == ID);
                    if (Bico != null)
                    {
                        if (Status != null) Bico.ComandoAtuar = (bool)Status;
                        if (Valor != null) Bico.AnguloControle = (int)Valor;
                        (id_num, posicao, data) = Bico.ProtocoloComando();
                    }
                    break;
                case S_Code.sSRV:
                    var Servo = DispAtu?.Dados?.Servos?.FirstOrDefault(x => x.ID == ID && x.Inicializado);
                    if (Servo != null)
                    {
                        if (Valor != null) Servo._AnguloControle = (int)Valor;
                        (id_num, posicao, data) = Servo.ProtocoloComando();
                    }
                    break;
            }

            DispAtu.AdicionarMensagemFila(id_num, posicao, posicao, data, false);

        }

        public static void EnviarComandoSensoriamento(S_Code Componente, string ID, dynamic Status, bool Forcar = false)
        {
            var DispSen = Variaveis.OperacaoEmAndamento.DispSen;
            if (!DispSen.Dados.Conectado)
                return;

            if (!Forcar)
            {
                bool componente_em_uso = Variaveis.OperacaoEmAndamento.Parametros.ModulosMandatorios.Any(x => x.Dispositivo == T_Code.Sen && (x.ComponentesEmUso?.ContainsKey(ID) ?? false) && x.ComponentesEmUso[ID].Item2);
                if (!componente_em_uso)
                {
                    // Impede execucao de comandos que liguem componentes quando estiver marcado para nao utilizar
                    if (Componente == S_Code.sRLE && ((Estado)Status) == Estado.Ligado) return;
                    if (Componente == S_Code.sSRV && ((int)Status) > 0) return;
                }
            }

            int id_num = -1;
            CanMessagePosicaoDados posicao = CanMessagePosicaoDados.Command1;
            byte[] data = null;

            switch (Componente)
            {
                case S_Code.sMOD:
                    if (DispSen?.Dados != null && Status == true)
                    {
                        (id_num, posicao, data) = DispSen.Dados.ProtocoloComandoReset();
                    }
                    break;
                case S_Code.sLED:
                    var Sinaleiro = DispSen?.Dados?.Sinaleiros?.FirstOrDefault(x => x.ID == ID && x.Inicializado);
                    if (Sinaleiro != null)
                    {
                        Sinaleiro.Comportamento = (SensorSinaleiroComportamentoModel)Status;
                        (id_num, posicao, data) = Sinaleiro.ProtocoloComando();
                    }
                    break;
                case S_Code.sRLE:
                    var Rele = DispSen?.Dados?.Reles?.FirstOrDefault(x => x.ID == ID && x.Inicializado);
                    if (Rele != null)
                    {
                        Rele.StatusControle = (Estado)Status;
                        (id_num, posicao, data) = Rele.ProtocoloComando();
                    }
                    break;
                case S_Code.sSRV:
                    var ServoFreio = DispSen?.Dados?.Servos?.FirstOrDefault(x => x.ID == ID && x.Inicializado);
                    if (ServoFreio != null)
                    {
                        ServoFreio._AnguloControle = (int)Status;
                        (id_num, posicao, data) = ServoFreio.ProtocoloComando();
                    }
                    break;
            }

            DispSen.AdicionarMensagemFila(id_num, posicao, posicao, data, false);
        }

        private static void AtualizaConsole(string Mensagem)
        {
            if (frmInstancial.frmJoystick.lblConsole.InvokeRequired)
            {
                frmInstancial.frmJoystick.lblConsole.Invoke((MethodInvoker)delegate
                {
                    frmInstancial.frmJoystick.lblConsole.Text = Mensagem;
                });
            }
            else
            {
                frmInstancial.frmJoystick.lblConsole.Text = Mensagem;
            }
        }

        private static bool VerificaComandoValidoSonar(Keys Comando)
        {
            List<Keys> ComandosImpedidos = new List<Keys>();
            var Sonar = Variaveis.OperacaoEmAndamento.Sensoriamento.OperadorVisual;
            var DeveParar = Sonar?.Resumo?.DeveParar ?? false;
            if (DeveParar)
            {
                ComandosImpedidos.Add(Keys.Up);
                /*switch (Sonar.Resumo.DirecaoDesvio)
                {
                    case Direcao.Parado:
                        ComandosImpedidos.Add(Keys.Up);
                        break;
                    case Direcao.Esquerda:
                        ComandosImpedidos.Add(Keys.Right);
                        break;
                    case Direcao.Direita:
                        ComandosImpedidos.Add(Keys.Left);
                        break;
                }*/
            }

            if (UltrasonicA05Service.Iniciado)
            {
                if (UltrasonicA05Service.DadosLeitura.Configuracao._paradaFrontal)
                {
                    ComandosImpedidos.Add(Keys.Up);
                }
                if (UltrasonicA05Service.DadosLeitura.Configuracao._paradaTraseira)
                {
                    ComandosImpedidos.Add(Keys.Down);
                }
                if (UltrasonicA05Service.DadosLeitura.Configuracao._paradaEsquerda)
                {
                    ComandosImpedidos.Add(Keys.Left);
                }
                if (UltrasonicA05Service.DadosLeitura.Configuracao._paradaDireita)
                {
                    ComandosImpedidos.Add(Keys.Right);
                }
            }

            return !ComandosImpedidos.Any(x => x == Comando);
        }


    }

    public class ComandosDispositivo
    {
        public DateTime Momento { get; set; }
        public Keys Comando { get; set; }
        public T_Code Dispositivo { get; set; }
        public string IDcontrolar { get; set; }
    }

    public class DeParaComandos
    {
        public BotoesJoystick botaoJoy { get; set; }
        public Keys key { get; set; }
        public T_Code Dispositivo { get; set; }
        public bool PararAoSoltar { get; set; }
        public string IDcontrolar { get; set; }
    }

    public enum ComparativoAnalogico
    {
        Igual,
        Maior,
        Menor
    }

    public class BotoesPressionadosValores
    {
        public BotoesJoystick botao { get; set; }
        public int valor { get; set; }
    }

}