// Dispositivo: Atuador
// Versão Firmware: 9
// Ultima atualização: 10/04/2025
// Atualização: Implementado comunicacao CAN

#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\CanService.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\EepromService.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\I2CService.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Utils.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Pinout.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\ReleModel.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\BombaPressurizadoraModel.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SensorFluxoModel.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SensorMassaModel.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SensorPressaoModel.h"


// Configurações do CAN
uint8_t CanID = 0x12;
CanService canService = CanService(CanID);

#define D_Code Atu
int VERSION = 9;
const String Mod_ID = "Atu";

#define _pinoLED RGB_BUILTIN

bool Conectado = false;
bool Conectar = false;

int _TaxaAmostragem = 1000;
volatile bool RequisitarDados = false;
int latenciaLoop = 0;
int qtdEnviosStatus = 0;
int maxEnviosStatus = 5;

std::vector<Rele*> listaBicos;
std::vector<BombaPressurizadora*> listaBombas;
std::vector<SensorFluxo*> listaSensoresFluxo;
std::vector<SensorMassa*> listaSensoresMassa;
std::vector<SensorPressao*> listaSensoresPressao;


void setup() {
  delay(5000);

  // Registrar callback para processar mensagens CAN recebidas
  canService.setReceiveCallback([](int packetSize, int senderId, byte funcCode, byte* data, int dataLength) {
    std::vector<uint8_t> fullData;
    fullData.push_back(funcCode);  // Primeiro byte é sempre o funcCode
    for (int i = 0; i < dataLength; i++) {
      fullData.push_back(data[i]);
    }

    ProcessarFrameCAN(fullData);
  });

  canService.begin();
}

void loop() {
  unsigned long startLoop = millis();

  if (!RequisitarDados) {
    enviarLogsPeriodicamente();
  }

  canService.loop();

  for (auto* bomba : listaBombas) {
    bomba->Loop();
  }
  for (auto* sensor : listaSensoresFluxo) {
    sensor->Loop();
  }

  delay(1);

  latenciaLoop = millis() - startLoop;
}

// Função para enviar logs do sensor periodicamente
unsigned long ultimoEnvioLogs = 0;
const unsigned long intervaloEnvioLogs = 1000;  // 1 segundo
void enviarLogsPeriodicamente() {
  unsigned long tempoAtual = millis();
  // Verifica se o intervalo de 5 segundos já passou
  if (tempoAtual - ultimoEnvioLogs >= intervaloEnvioLogs) {
    ultimoEnvioLogs = tempoAtual;  // Atualiza o tempo do último envio

    enviarDadosSensores();  // Chama o método que já envia os dados do sensor via MQTT
  }
}

void enviarDadosSensores() {
  if (Conectado) {
    PrintTela("Atualizando dados dos sensores...");

    bool enviarStatus = qtdEnviosStatus >= maxEnviosStatus;

    // Aferir dados dos sensores
    for (auto* bico : listaBicos) {
      if (bico != nullptr) {
        bico->RequisitarDados();
        if (enviarStatus) {
          EnviarDadosCAN(bico->MontarMensagemCAN(CanMessagePosicaoDados::Status));
        }
        EnviarDadosCAN(bico->MontarMensagemCAN(CanMessagePosicaoDados::Dados1));
      }
    }

    // Aferir dados dos sensores
    for (auto* sensor : listaSensoresFluxo) {
      if (sensor != nullptr) {
        sensor->RequisitarDados();
        if (enviarStatus) {
          EnviarDadosCAN(sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status));
        }
        EnviarDadosCAN(sensor->MontarMensagemCAN(CanMessagePosicaoDados::Dados1));
      }
    }

    // Aferir dados dos sensores
    for (auto* sensor : listaSensoresMassa) {
      if (sensor != nullptr) {
        sensor->RequisitarDados();
        if (enviarStatus) {
          EnviarDadosCAN(sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status));
        }
        EnviarDadosCAN(sensor->MontarMensagemCAN(CanMessagePosicaoDados::Dados1));
      }
    }

    // Aferir dados dos sensores
    for (auto* sensor : listaSensoresPressao) {
      if (sensor != nullptr) {
        sensor->RequisitarDados();
        if (enviarStatus) {
          EnviarDadosCAN(sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status));
        }
        EnviarDadosCAN(sensor->MontarMensagemCAN(CanMessagePosicaoDados::Dados1));
      }
    }

    // Aferir dados dos sensores
    for (auto* bomba : listaBombas) {
      if (bomba != nullptr) {
        bomba->RequisitarDados();
        if (enviarStatus) {
          EnviarDadosCAN(bomba->MontarMensagemCAN(CanMessagePosicaoDados::Status));
        }
        EnviarDadosCAN(bomba->MontarMensagemCAN(CanMessagePosicaoDados::Dados1));
      }
    }

    if (enviarStatus) {
      qtdEnviosStatus = 0;
    }
    else {
      qtdEnviosStatus++;
    }

  }
}


void EnviarDadosCAN(std::vector<uint8_t> data) {
  if (data.size() > 0) {
    canService.enviarMensagem(data);
  }
}


void ProcessarFrameCAN(std::vector<uint8_t> data) {
  if (data.size() < 1) return;

  F_Code funcao = (F_Code)data[0];

  switch (funcao) {
    case ChkTx: ProcessarChk(); break;
    case ReqTx: ProcessarReq(); break;
    case CmdTx: ProcessarCmd(data); break;
    case CfgTx: ProcessarCfg(data); break;
  }
}

void ProcessarChk() {
  std::vector<uint8_t> data = canService.MontarFrameChk(D_Code, Conectado, VERSION);
  EnviarDadosCAN(data);
}

void ProcessarReq() {
  enviarDadosSensores();
}

void ProcessarCfg(std::vector<uint8_t> data) {
  if (data.size() < 4) return;
  std::vector<uint8_t> _chkRx;
  uint8_t id = data[1];
  CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[2];
  S_Code componente = (S_Code)data[3];
  switch (componente) {
    case sMOD: {
      switch (posicao) {
        case CanMessagePosicaoDados::Config1: {
          if (data.size() < 7) return;
          bool conectar = data[4] == 1;
          int taxa = data[5] * 10;
          bool requisitar = data[6] == 1;
          _TaxaAmostragem = taxa;
          RequisitarDados = requisitar;
          Conectar = conectar;
          break;
        }
        case CanMessagePosicaoDados::Config2: {
          if (data.size() < 6) return;
          int pinoSDA = data[4];
          int pinoSCL = data[5];
          if (pinoSDA != PINO_INVALIDO && pinoSCL != PINO_INVALIDO) {
            I2CService::DefinirPinos(pinoSDA, pinoSCL);
            I2CService::IniciarI2C();
          }
          vTaskDelay(pdMS_TO_TICKS(100));
          Conectado = Conectar;
          _chkRx = canService.MontarFrameChk(D_Code, Conectado, VERSION);
          LimparListasComponentes();
          PrintTela("Configuracao do modulo concluida");
          break;
        }
      }
      break;
    }
    case sBIC: {
      _chkRx = Rele::ConfigurarRele(listaBicos, data, Mod_ID);
      break;
    }
    case sFLX: {
      _chkRx = SensorFluxo::ConfigurarSensor(listaSensoresFluxo, data, Mod_ID);
      break;
    }
    case sPRS: {
      _chkRx = SensorPressao::ConfigurarSensor(listaSensoresPressao, data, Mod_ID);
      break;
    }
    case sMAS: {
      _chkRx = SensorMassa::ConfigurarSensor(listaSensoresMassa, data, Mod_ID);
      break;
    }
    case sBMB: {
      _chkRx = BombaPressurizadora::ConfigurarBomba(listaBombas, data, Mod_ID, listaSensoresPressao);
      break;
    }
  }

  EnviarDadosCAN(_chkRx);
}

void ProcessarCmd(std::vector<uint8_t> data) {
  if (data.size() < 4) return;
  uint8_t id = data[1];
  CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[2];
  S_Code componente = DetectarComponente(id);
  switch (componente) {
    case sBIC: {
      Rele::ComandarRele(listaBicos, data, id);
      break;
    }
    case sBMB: {
      BombaPressurizadora::ComandarBomba(listaBombas, data, id);
      break;
    }
    default:
      break;
  }
}

void LimparListasComponentes() {
  for (auto* bico : listaBicos) {
    bico->Desligar();
    PrintTela("Bico " + bico->_ID + " desligado.");
    delete bico;
  }
  listaBicos.clear();

  for (auto* bomba : listaBombas) {
    bomba->Desligar();
    PrintTela("Bomba Pressurizadora " + bomba->_ID + " desligada.");
    delete bomba;
  }
  listaBombas.clear();

  for (auto* sensor : listaSensoresFluxo) {
    sensor->Desligar();
    PrintTela("Sensor de Fluxo " + sensor->_ID + " desligado.");
    delete sensor;
  }
  listaSensoresFluxo.clear();

  for (auto* sensor : listaSensoresMassa) {
    sensor->Desligar();
    PrintTela("Sensor de Massa " + sensor->_ID + " desligado.");
    delete sensor;
  }
  listaSensoresMassa.clear();

  for (auto* sensor : listaSensoresPressao) {
    sensor->Desligar();
    PrintTela("Sensor de Pressao " + sensor->_ID + " desligado.");
    delete sensor;
  }
  listaSensoresPressao.clear();
}

S_Code DetectarComponente(uint8_t idNum) {
  if (idNum == canService.ID_Num_sMOD) return S_Code::sMOD;
  for (auto* s : listaBicos) {
    if (s->ID_Num == idNum) return S_Code::sBIC;
  }
  for (auto* s : listaBombas) {
    if (s->ID_Num == idNum) return S_Code::sBMB;
  }
  for (auto* s : listaSensoresFluxo) {
    if (s->ID_Num == idNum) return S_Code::sFLX;
  }
  for (auto* s : listaSensoresMassa) {
    if (s->ID_Num == idNum) return S_Code::sMAS;
  }
  for (auto* s : listaSensoresPressao) {
    if (s->ID_Num == idNum) return S_Code::sPRS;
  }
  return S_Code::sVZO;
}