agrobot_base/AgroBase/AgroBase/Services/CANService.cs

using AgroBase.Models;
using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using static AgroBase.Models.Enums;
using static WaveshareCANFDService;

namespace AgroBase.Services
{
    public class CanMessage
    {
        public DateTime Momento { get; set; }
        public T_Code Dispositivo { get; set; }
        public byte IdTx { get; set; }
        public byte IdRx { get; set; }
        public byte funcCodeTx { get; set; }
        public byte funcCodeRx { get; set; }
        public byte[] DataTx { get; set; }
        public byte[] DataRx { get; set; }
        public bool Enviado { get; set; }
        public DateTime EnviadoEm { get; set; }
        public bool ComResposta { get; set; }
        public bool Respondido { get; set; }
        public DateTime RespondidoEm { get; set; }

        public string ChaveInterna { get; set; }
    }

    public enum CanMessagePosicaoDados
    {
        Status = 0,
        DadosAll = 1, 
        Dados1 = 2,
        Dados2 = 3,
        Dados3 = 4,
        Dados4 = 5,
        Dados5 = 6,
        Dados6 = 7,
        Config1 = 51,
        Config2 = 52,
        Config3 = 53,
        Config4 = 54,
        Config5 = 55,
        Command1 = 101,
        Command2 = 102,
        Command3 = 103,
        Command4 = 104,
        Command5 = 105,
        Command6 = 106,
        Command7 = 107,
        Command8 = 108,
        Command9 = 109,
        Command10 = 110,
    }

    public interface ICanMessageHandler
    {
        T_Code Dispositivo { get; set; }
        void ProcessarMensagem(CanMessage mensagem);
    }

    public class CanService : IDisposable
    {
        public static WaveshareCANFDService Connector = new WaveshareCANFDService(0);
        public static string _portName = "CAN-FD";
        public static bool IsConnected { get; private set; }
        public static bool Iniciado
        {
            get
            {
                return SerialService.DispositivosCan.Any(d => SerialService.DispositivosMapeados.Any(x => x.Conectado && x.Dispositivo == d));
            }
        }
        private static AsyncTaskTimerModel tmrEnviarCAN;
        private static AsyncTaskTimerModel tmrOuvirCAN;
        private static AsyncTaskTimerModel tmrProcessarCAN;
        private static StringBuilder SerialCANBuffer = new StringBuilder();
        private static ConcurrentQueue<(byte, byte[])> FramesRecebidos = new ConcurrentQueue<(byte, byte[])>();
        private static readonly ConcurrentDictionary<byte, ICanMessageHandler> _roteadores = new ConcurrentDictionary<byte, ICanMessageHandler>();
        private static byte? _ultimoIdEnviado = null;
        private static List<CanMessage> MensagensPendentes = new List<CanMessage>();
        private static readonly object _LockMensagens = new object();
        private static bool DebugMode = false;

        private static void MostrarLog(string message, byte id)
        {
            if (DebugMode && new List<byte>() { 0x01 }.Contains(id))
            {
                Console.WriteLine($"[CAN] {message}");
            }
        }

        public static void RegistrarHandler(byte endereco_rx, ICanMessageHandler handler)
        {
            _roteadores[endereco_rx] = handler;
            MostrarLog($"Roteador registrado: {endereco_rx.ToString("X")}", endereco_rx);
        }

        public static bool InicializarCanal()
        {
            bool canIniciado = Connector.IsOpen;

            if (!canIniciado)
            {
                canIniciado = Connector.Open((VCI_CAN_OBJ frame) =>
                {
                    byte[] dataRx = new byte[frame.DataLen];
                    Array.Copy(frame.Data, dataRx, frame.DataLen);
                    FramesRecebidos.Enqueue(((byte)frame.ID, dataRx));
                });
            }
            else
            {
                return true;
            }

            if (!canIniciado)
            {
                MostrarLog($"Erro ao iniciar o conversor CAN FD no canal {Connector.Canal}.", 0);
            }
            else
            {
                MostrarLog($"Conversor CAN FD iniciado com sucesso no canal {Connector.Canal}.", 0);
            }

            if (canIniciado || !IsConnected)
            {
                tmrEnviarCAN?.Dispose();
                tmrEnviarCAN = new AsyncTaskTimerModel("tmrEnviarCAN", EnviarCAN, 5);
                tmrEnviarCAN.Start();

                tmrProcessarCAN?.Dispose();
                tmrProcessarCAN = new AsyncTaskTimerModel("tmrProcessarCAN", ProcessarCAN, 10);
                tmrProcessarCAN.Start();

                IsConnected = true;
            }

            return canIniciado;
        }

        private static async Task ProcessarCAN()
        {
            int mensagensParaProcessar = Math.Min(FramesRecebidos.Count, 10);

            for (int i = 0; i < mensagensParaProcessar; i++)
            {
                if (FramesRecebidos.TryDequeue(out (byte, byte[]) frame))
                {
                    ProcessarFrameCAN(frame);
                }
                else
                {
                    // A fila ficou vazia antes de atingir o limite
                    break;
                }
            }
            LimparMensagensAntigas();
        }

        private static void ProcessarFrameCAN((byte, byte[]) frame)
        {
            byte id = frame.Item1;
            byte[] data = frame.Item2;

            try
            {
                MostrarLog($"Processando frame: {BitConverter.ToString(data)}", id);

                if (_roteadores.TryGetValue(id, out ICanMessageHandler handler))
                {

                }

                T_Code dispositivo = handler?.Dispositivo ?? T_Code.Vzo;
                byte fCodeRx = data[0];
                byte parametroRx = data[1];
                string chave = GerarChaveMensagem(dispositivo, id, data);

                CanMessage mensagem = null;
                lock (_LockMensagens)
                {
                    mensagem = MensagensPendentes.FirstOrDefault(x => x.Enviado && !x.Respondido && x.Dispositivo == x.Dispositivo && x.IdRx == id && x.DataTx[1] == data[1]);
                }
                if (mensagem == null)
                {
                    mensagem = new CanMessage { Dispositivo = dispositivo, IdTx = id, IdRx = id, funcCodeRx = fCodeRx, DataRx = data, ChaveInterna = chave };
                }
                mensagem.DataRx = data;
                mensagem.Respondido = true;
                mensagem.RespondidoEm = DateTime.Now;

                if ((handler?.Dispositivo ?? T_Code.Vzo) != T_Code.Vzo)
                {
                    handler.ProcessarMensagem(mensagem);
                }
                else
                {
                    MostrarLog($"Roteador não registrado! ID_Num não tratado: 0x{id:X2}, fCodeRx: 0x{fCodeRx:X2}, frame: {frame}", id);
                }
            }
            catch (Exception ex)
            {
                MostrarLog($"Erro ao processar frame! Frame: {frame}, Erro: {ex.Message}", id);
            }
        }

        public static void AdicionarMensagemNaFila(T_Code Dispositivo, byte idTx, byte idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true)
        {
            if (!Connector.IsOpen) return;

            var dados = new List<byte>() { funcCodeTx };

            if (payload != null)
                dados.AddRange(payload);

            if (Dispositivo == T_Code.Mks)
            {
                byte crc = CalcularChecksum(idTx, dados.ToArray());
                dados.Add(crc);
            }

            var novaMensagem = new CanMessage
            {
                Momento = DateTime.Now,
                Dispositivo = Dispositivo,
                IdTx = idTx,
                IdRx = idRx,
                funcCodeTx = funcCodeTx,
                funcCodeRx = funcCodeRx,
                DataTx = dados.ToArray(),
                ComResposta = get,
                ChaveInterna = GerarChaveMensagem(Dispositivo, idRx, dados.ToArray())
            };

            bool mensagemNaFila = false;

            lock (_LockMensagens)
            {
                mensagemNaFila = MensagensPendentes.Any(x => (!x.Enviado || (x.ComResposta && x.Enviado && !x.Respondido)) && x.IdTx == idTx && x.IdRx == idRx && x.DataTx.SequenceEqual(dados.ToArray()));
                if (!mensagemNaFila)
                {
                    MensagensPendentes.Add(novaMensagem);
                    MostrarLog($"Mensagem adicionada na fila. ID: {novaMensagem.IdTx}, Data: {FuncoesGlobais.ConverterComandoBytesParaTexto(novaMensagem.DataTx)}", idTx);
                }
                else
                {
                    MostrarLog($"Mensagem já pendente e idêntica. ID: {novaMensagem.IdTx}, Data: {FuncoesGlobais.ConverterComandoBytesParaTexto(novaMensagem.DataTx)}", idTx);
                }
            }
        }

        private static CanMessage ObterProximaMensagem()
        {
            List<CanMessage> mensagensPendentes;

            lock (_LockMensagens)
            {
                mensagensPendentes = MensagensPendentes
                    .Where(x => !x.Enviado)
                    .ToList();
            }

            if (!mensagensPendentes.Any())
                return null;

            // Primeiro tenta pegar mensagens com Get == false
            var mensagensPrioritarias = mensagensPendentes
                .Where(x => !x.ComResposta)
                .ToList();

            // Se não houver mensagens prioritárias, considera todas
            var mensagensParaProcessar = mensagensPrioritarias.Any()
                ? mensagensPrioritarias
                : mensagensPendentes;

            // Agrupa por IdTx e ordena os grupos
            var idsDisponiveis = mensagensParaProcessar
                .Select(x => x.IdTx)
                .Distinct()
                .OrderBy(x => x)
                .ToList();

            byte proximoId;

            if (!_ultimoIdEnviado.HasValue || !idsDisponiveis.Contains(_ultimoIdEnviado.Value))
            {
                proximoId = idsDisponiveis.First();
            }
            else
            {
                var index = idsDisponiveis.IndexOf(_ultimoIdEnviado.Value);
                proximoId = (index + 1 < idsDisponiveis.Count) ? idsDisponiveis[index + 1] : idsDisponiveis.First();
            }

            // Seleciona a mensagem mais antiga com esse IdTx
            var proximaMsg = mensagensParaProcessar
                .Where(x => x.IdTx == proximoId)
                .OrderBy(x => x.Momento)
                .FirstOrDefault();

            return proximaMsg;
        }

        private static async Task EnviarCAN()
        {
            if (!Connector.IsOpen) return;

            CanMessage msg = ObterProximaMensagem();
            
            if (msg != null)
            {
                string frame = CriarFrame(msg);

                if (_ultimoIdEnviado.HasValue && msg.IdTx == _ultimoIdEnviado.Value)
                {
                    await Task.Delay(5);
                }

                if (msg.Dispositivo == T_Code.Mks)
                {
                    await Task.Delay(10);
                }

                if (!EnviarComando(frame, msg))
                {
                    MostrarLog("Erro ao enviar frame", msg.IdTx);
                }

                msg.Enviado = true;
                msg.EnviadoEm = DateTime.Now;
                _ultimoIdEnviado = msg.IdTx;
            }
        }

        private static string CriarFrame(CanMessage msg)
        {
            string frame = $"t{msg.IdTx:X3}{msg.DataTx.Length}";
            foreach (byte b in msg.DataTx)
                frame += b.ToString("X2");

            frame += "\r";

            return frame;
        }

        private static bool EnviarComando(string comando, CanMessage msg = null)
        {
            if (Connector.IsOpen)
            {
                bool sucesso = Connector.Send(msg.IdTx, msg.DataTx);

                if (sucesso)
                {
                    MostrarLog($"Frame enviado: {comando}", msg.IdTx);
                }
                else
                {
                    MostrarLog($"Erro ao enviar frame: {comando}", msg.IdTx);
                }
                return sucesso;
            }

            return false;
        }

        private static void LimparMensagensAntigas()
        {
            var agora = DateTime.Now;

            lock (_LockMensagens)
            {
                List<CanMessage> mensagens = MensagensPendentes
                    .Where(x => 
                        (!x.ComResposta && x.Enviado) || 
                        (x.ComResposta && x.Respondido) || 
                        (x.ComResposta && x.Enviado && !x.Respondido && x.EnviadoEm < agora.AddSeconds(-1)) ||
                        (x.Momento < agora.AddSeconds(-30))
                    )
                    .ToList();
                foreach (var item in mensagens)
                {
                    MensagensPendentes.Remove(item);
                }
            }
        }

        public static void Fechar()
        {
            tmrOuvirCAN?.Dispose();
            tmrEnviarCAN?.Dispose();

            lock (_LockMensagens)
            {
                MensagensPendentes.Clear();
            }

            SerialCANBuffer.Clear();
            FramesRecebidos = new ConcurrentQueue<(byte, byte[])>();
            _roteadores.Clear();
            _ultimoIdEnviado = null;

            Connector.Close();

            IsConnected = false;
        }

        public void Dispose()
        {
            Fechar();
        }

        private static byte CalcularChecksum(byte id, byte[] data)
        {
            int soma = id;
            foreach (var b in data)
                soma += b;
            return (byte)(soma & 0xFF);
        }

        private static string GerarChaveMensagem(T_Code Dispositivo, byte Id, byte[] data)
        {
            byte parametro = 0x00;

            // Para dispositivos que não sejam MKS, usar DataTx[1] se existir
            if (Dispositivo != T_Code.Mks && (data?.Length ?? 0) > 1)
            {
                parametro = data[1];
            }

            return $"{Id}_{data[0]}_{parametro}";
        }

    }
}