// Dispositivo: Direcional
// Versão Firmware: 5
// Ultima atualização: 04/04/2024
// Atualização: Atualização do modelo de pinout

#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Pinout.h"
#define D_Code Dir
#define VERSION 5

#define _pinoLED RGB_BUILTIN

long _baudRate = 115200;

bool Conectado = false;
bool AA_Ref000 = false;
bool AA_Ref090 = false;
bool AA_Ref270 = false;

int _TaxaAmostragem;
int FrequenciaMaxima = 10000;
double _Margem = 0.5;

int dutyCycle = 512;
double _frequencia = 4000;

PinoModel _pinoGeral;

class Motor {
  public:
    // Construtor
    Motor(String desc) {
      _ID = desc;
    }

    // Definições
    String _ID;
    int _canal;
    PinoModel _pinoENA;
    PinoModel _pinoDIR;
    PinoModel _pinoPUL;
    PinoModel _pinoEncoderA;
    PinoModel _pinoEncoderB;
    PinoModel _pinoRef000;
    PinoModel _pinoRef090;
    PinoModel _pinoRef270;
    
    bool Iniciado = false;
    bool Testando = false;
    bool Referenciando = false;

    // Consumo
    double _Angulo;
    double _AgRef;
    Sentido _Sentido;
    
    // Entrada de dados
    double _Angulo_SP;
    double _Angulo_Offset = 0;
    Sentido _Sentido_SP = Parado;
    bool _MotorLigado = false;
    bool _Estabilizar = true;
    bool _Referenciar = true;

    bool E0 = false;
    bool Ag0 = false;
    bool _Ag0 = false;
    bool E90 = false;
    bool Ag90 = false;
    bool _Ag90 = false;
    bool E270 = false;
    bool Ag270 = false;
    bool _Ag270 = false;
    bool Referenciado = false;
    int RefMax = 90;
    int RefMin = -90;
    int ulEncoderA = 0;
    int ulEncoderB = 0;
    double GrausPino = 0;
    bool EA_M = false;
    bool EB_M = false;


    void Inicializar() {
      if (Iniciado) {
        EnviarDadosSerial(_ID + " ja inicializado");
        EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }

      if (_pinoPUL.Definido()) {
        AtualizarPWM();
      }
      _pinoENA.Conectar(true);
      _pinoDIR.Conectar(false);
      _pinoEncoderA.Conectar();
      ulEncoderA = _pinoEncoderA.get();
      EA_M = false;
      _pinoEncoderB.Conectar();
      ulEncoderB = _pinoEncoderB.get();
      EB_M = false;
      _pinoRef000.Conectar();
      Ag0 = _pinoRef000.get() == 1;
      _Ag0 = Ag0;
      _pinoRef090.Conectar();
      Ag90 = _pinoRef090.get() == 1;
      _Ag90 = Ag90;
      _pinoRef270.Conectar();
      Ag270 = _pinoRef270.get() == 1;
      _Ag270 = Ag270;
      
      xTaskCreatePinnedToCore(&Motor::RampaTaskWrapper, "RampaTask", 5000, this, 20 - _canal, &RampaTaskHandle, tskNO_AFFINITY);


      E0 = false;
      E90 = false;
      E270 = false;

      ResetRef(_Referenciar);

      EnviarDadosSerial(_ID + " Iniciado");

      Iniciado = true;

      EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
    }

    void ResetRef(bool _Ref) {
      _Angulo = 0;
      GrausPino = 0;
      _Sentido_SP = Parado;
      _Sentido = Parado;
      Referenciado = !_Ref;
      Referenciando = _Ref;
      _MotorLigado = _Ref;
    }

    void AtualizarPWM() {
      ledcSetup(_canal, _frequencia, 10);
      ledcAttachPin(_pinoPUL.num, _canal);
      ledcWrite(_canal, dutyCycle);
    }

    void Desligar() {
      if (!Iniciado) {
        EnviarDadosSerial(_ID + " nao esta inicializado");
        EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }

      // Parar a execução das tarefas
      vTaskDelete(RampaTaskHandle);

      // Redefinir as configurações para os valores iniciais
      _pinoENA.Desconectar();
      _pinoDIR.Desconectar();
      _pinoPUL.Desconectar();
      _pinoEncoderA.Desconectar();
      _pinoEncoderB.Desconectar();
      _pinoRef000.Desconectar();
      _pinoRef090.Desconectar();
      _pinoRef270.Desconectar();
      
      
      EnviarDadosSerial(_ID + " Desligado");

      Iniciado = false;

      EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
    }

    void Testar() {
      Testando = true;

      Testando = false;
    }

  

  private:

    TaskHandle_t RampaTaskHandle = NULL;
    static void RampaTaskWrapper(void *pvParameters) {
      Motor *motor = static_cast<Motor*>(pvParameters);
      motor->RampaTask();
    }

    void RampaTask() {
      unsigned long previousMillisRampa = millis();
      unsigned long previousMillisAngulo = millis();

      while (1)
      {
        if (!Conectado) {
          // Caso não esteja conectado, aguarda 1 segundo até a próxima verificação, liberando uso de CPU
          vTaskDelay(1000);
          continue;
        }

        AferirPosicao();

        if (Referenciando) {
          ReferenciarMotor();
        }
        else {
          VerificarAnguloSP();
        }

        Atualizar();

        AferirAnguloAbsoluto();

        unsigned long currentMillis = millis();
        /*if (currentMillis - previousMillisAngulo >= 100) {
          previousMillisAngulo = currentMillis;

          AferirAnguloAbsoluto();
        }*/

        if (currentMillis - previousMillisRampa >= _TaxaAmostragem) {
          previousMillisRampa = currentMillis;

          EnviarDadosSerial(MontarProtocoloSensor(sENC, _ID, MontarDadosProtocoloEncoder(_Sentido, _Angulo, Ag0, Ag90, Ag270, Referenciando, Referenciado)));
        }

        vTaskDelay(1);

      }
    }

    void Atualizar() {
      _pinoENA.set(!_MotorLigado);
      _pinoDIR.set(_Sentido_SP != Horario);
    }

    void AferirPosicao() {
      if (_pinoEncoderA.Definido() && _pinoEncoderB.Definido()) {
        int leituraEncoderA = _pinoEncoderA.get();
        int leituraEncoderB = _pinoEncoderB.get();

        if (!EA_M) {
          EA_M = leituraEncoderA != ulEncoderA;
          if (EA_M) {
            _Sentido = leituraEncoderA != leituraEncoderB ? Horario : Antihorario;
          }
        }
        if (!EB_M) {
          EB_M = leituraEncoderB != ulEncoderB;
        }


        if (EA_M && EB_M) {

          if (_Sentido == Horario) {
            _Angulo = _Angulo + _Margem;
            _AgRef = _AgRef + _Margem;
          } else if (_Sentido == Antihorario) {
            _Angulo = _Angulo - _Margem;
            _AgRef = _AgRef - _Margem;
          }

          EA_M = false;
          EB_M = false;
        }

        ulEncoderA = leituraEncoderA;
        ulEncoderB = leituraEncoderB;
      }
    }


    void AferirAnguloAbsoluto() {
      _Ag0 = _pinoRef000.get() == 1;
      _Ag90 = _pinoRef090.get() == 1;
      _Ag270 = _pinoRef270.get() == 1;

      if (GrausPino == 0) {
          if (_pinoRef000.Definido()) {
            if (_Ag0 == true && Ag0 == false && E0 == false) { // entrou no pino 0
              _AgRef = 0;
              //Serial.println("Entrou 0");
              E0 = true;
            }
            else if (_Ag0 == false && Ag0 == true && _AgRef != 0 && E0 == true) { // saiu do pino 0
              GrausPino = (_AgRef < 0 ? -1.0 : 1.0) * _AgRef;
              Referenciado = true;
              //Serial.println("Saiu 0");
              //Serial.println(GrausPino);
              E0 = false;
            }
          }
          if (_pinoRef090.Definido()) {
            if (_Ag90 == true && Ag90 == false && E90 == false) { // entrou no pino 90
              _AgRef = 0;
              //Serial.println("Entrou 90");
              E90 = true;
            }
            else if (_Ag90 == false && Ag90 == true && E90 == true) { // saiu do pino 90
              GrausPino = (_AgRef < 0 ? -1.0 : 1.0) * _AgRef;
              Referenciado = true;
              //Serial.println("Saiu 90");
              E90 = false;
            }
          }
          if (_pinoRef270.Definido()) {
            if (_Ag270 == true && Ag270 == false && E270 == false) { // entrou no pino 270
              _AgRef = 0;
              //Serial.println("Entrou 270");
              E270 = true;
            }
            else if (_Ag270 == false && Ag270 == true) { // saiu do pino 270
              GrausPino = (_AgRef < 0 ? -1.0 : 1.0) * _AgRef;
              Referenciado = true;
              //Serial.println("Saiu 270");
              E270 = false;
            }
          }
      }

      if (GrausPino > 0) {
        if (Referenciando || AA_Ref000) {
          if (_Ag0 == true && Ag0 == false) { // entrou no pino 0
            _Angulo = CalculaAnguloPino(0, true);
          }
          else if (_Ag0 == false && Ag0 == true) { // saiu do pino 0
            _Angulo = CalculaAnguloPino(0, false);
          }
        }
        else if (Referenciando || AA_Ref090) {
          if (_Ag90 == true && Ag90 == false) { // entrou no pino 90
            _Angulo = CalculaAnguloPino(90, true);
          }
          else if (_Ag90 == false && Ag90 == true) { // saiu do pino 90
            _Angulo = CalculaAnguloPino(90, false);
          }
        }
        else if (Referenciando || AA_Ref270) {
          if (_Ag270 == true && Ag270 == false) { // entrou no pino 270
            _Angulo = CalculaAnguloPino(-90, true);
          }
          else if (_Ag270 == false && Ag270 == true) { // saiu do pino 270
            _Angulo = CalculaAnguloPino(-90, false);
          }
        }
      }

      if (_pinoRef000.Definido()) {
          Ag0 = _Ag0;
      }
      if (_pinoRef090.Definido()) {
          Ag90 = _Ag90;
      }
      if (_pinoRef270.Definido()) {
          Ag270 = _Ag270;
      }
    }

    double CalculaAnguloPino(double Inicio, bool Entrando) {
      double MeioPino = (GrausPino / 2.0);
      double Multiplicador = 1.0; 
      if (Entrando) {
        Multiplicador = (_Sentido == Horario ? -1.0 : 1.0);
      }
      else {
        Multiplicador = (_Sentido == Horario ? 1.0 : -1.0);
      }
      double Angulo = Inicio + (MeioPino * Multiplicador) + _Angulo_Offset;
      return Angulo;
    }

    void VerificarAnguloSP() {
      if (_MotorLigado) {
        if (_Angulo_SP > -1) {
          if (_Sentido_SP == Horario && _Angulo >= _Angulo_SP) {
            _MotorLigado = false;
          }
          else if (_Sentido_SP == Antihorario && _Angulo <= (_Angulo_SP * -1)) {
            _MotorLigado = false;
          }
        }
        else {
          if (Referenciado && (_Sentido_SP == Horario && _Angulo >= RefMax)) {
            _MotorLigado = false;
          }
          else if (Referenciado && (_Sentido_SP == Antihorario && _Angulo <= RefMin)) {
            _MotorLigado = false;
          }
        }
      }
      else {
        if (_Estabilizar && _Sentido_SP == Parado && _Angulo != _Angulo_SP) {
          _Sentido_SP = _Angulo > 0 ? Antihorario : Horario;
          _MotorLigado = true;
        }
      }
    }

    void ReferenciarMotor() {
      int AgP1 = RefMax / 2;
      int AgP2 = RefMax;

      if (Referenciado && (_Angulo >= -_Margem && _Angulo <= _Margem)) {
        Referenciando = false;
        _MotorLigado = false;
      }
      else {
        if (_Sentido_SP == Parado && _Angulo == 0) {
          _Angulo_SP = AgP1;
          _Sentido_SP = Horario;
        }
        else if (_Sentido_SP == Horario) {
          if (_Angulo > _Margem && Referenciado) {
            _Angulo_SP = AgP1;
            _Sentido_SP = Antihorario;
          }
          else if (_Angulo >= AgP1) {
            _Angulo_SP = AgP1;
            _Sentido_SP = Antihorario;
          }
          else if (_Angulo >= RefMax) {
            _Angulo_SP = AgP2;
            _Sentido_SP = Antihorario;
          }
        }
        else if (_Sentido_SP == Antihorario) {
          if (_Angulo < -_Margem && Referenciado) {
            _Angulo_SP = AgP1;
            _Sentido_SP = Horario;
          }
          else if (_Angulo <= -AgP1) {
            _Angulo_SP = AgP2;
            _Sentido_SP = Antihorario;
          }
          else if (_Angulo <= -AgP2) {
            _Angulo_SP = AgP2;
            _Sentido_SP = Horario;
          }
          else if (_Angulo <= RefMin) {
            _Angulo_SP = AgP2;
            _Sentido_SP = Horario;
          }
        }
      }
    }

};

Motor M1("ET");
Motor M2("EF");
Motor M3("DT");
Motor M4("DF");

Motor* MotorPorID(String ID) {
  Motor* _motor = 
    ID == "ET" ? &M1 : 
    ID == "EF" ? &M2 : 
    ID == "DT" ? &M3 : 
    ID == "DF" ? &M4 : 
    nullptr;
  return _motor;
}

void setup() {
  Serial.begin(_baudRate);
  //iniciarMCP(D_Code, 8, 18, 0x20, false);
}

void loop() {
  ProtocoloSerial protocolos[MAX_PROTOCOLS];
  int numProtocolos = LerBufferSerial(protocolos);

  // Processa cada protocolo após a leitura completa
  for (int i = 0; i < numProtocolos; i++) {
    ProcessarProtocolo(protocolos[i].protocolo, protocolos[i].funcao);
  }
}

void ProcessarProtocolo(String Protocolo, F_Code _funcao) {
  EnviarDadosSerial("OK");

  if (_funcao == Chk) {
      EnviarDadosSerial(MontarProtocoloVerificacao(D_Code, VERSION));
  }
  else if (_funcao == Cfg) {
      String ID = ((String)Protocolo[0] + (String)Protocolo[1]);
      bool Conectar = (String)Protocolo[3] == "1";
      if (ID == "MD") {
        _TaxaAmostragem = ((String)Protocolo[5] + (String)Protocolo[6] + (String)Protocolo[7] + (String)Protocolo[8] + (String)Protocolo[9]).toInt();
        AA_Ref000 = (String)Protocolo[11] == "1";
        AA_Ref090 = (String)Protocolo[13] == "1";
        AA_Ref270 = (String)Protocolo[15] == "1";
        TiposBarramentos _barramento = (TiposBarramentos)((String)Protocolo[17]).toInt();
        int _num = ((String)Protocolo[18] + (String)Protocolo[19]).toInt();
        _pinoGeral = PinoModel(_num, _barramento, _OUTPUT);

        if (_pinoGeral.Definido()) {
          _pinoGeral.set(true);
        }

        vTaskDelay(pdMS_TO_TICKS(100));
        
        Conectado = Conectar;

        EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD", Conectado ? "1" : "0"));
      }
      else {
        Motor* _motor = MotorPorID(ID);

        int canal = ((String)Protocolo[5]).toInt();
        bool motor_ativado = ((String)Protocolo[7]) == "1";
        bool estabilizar = ((String)Protocolo[9]) == "1";
        bool referenciar = ((String)Protocolo[11]) == "1";
        int _offset = ((String)Protocolo[13] + (String)Protocolo[14] + (String)Protocolo[15] + (String)Protocolo[16]).toInt();
        TiposBarramentos pulB = (TiposBarramentos)((String)Protocolo[18]).toInt();
        int pul = ((String)Protocolo[19] + (String)Protocolo[20]).toInt();
        TiposBarramentos dirB = (TiposBarramentos)((String)Protocolo[22]).toInt();
        int dir = ((String)Protocolo[23] + (String)Protocolo[24]).toInt();
        TiposBarramentos enaB = (TiposBarramentos)((String)Protocolo[26]).toInt();
        int ena = ((String)Protocolo[27] + (String)Protocolo[28]).toInt();
        TiposBarramentos encAB = (TiposBarramentos)((String)Protocolo[30]).toInt();
        int encoderA = ((String)Protocolo[31] + (String)Protocolo[32]).toInt();
        TiposBarramentos encBB = (TiposBarramentos)((String)Protocolo[34]).toInt();
        int encoderB = ((String)Protocolo[35] + (String)Protocolo[36]).toInt();
        TiposBarramentos ref0B = (TiposBarramentos)((String)Protocolo[38]).toInt();
        int ref0 = ((String)Protocolo[39] + (String)Protocolo[40]).toInt();
        TiposBarramentos ref90B = (TiposBarramentos)((String)Protocolo[42]).toInt();
        int ref90 = ((String)Protocolo[43] + (String)Protocolo[44]).toInt();
        TiposBarramentos ref270B = (TiposBarramentos)((String)Protocolo[46]).toInt();
        int ref270 = ((String)Protocolo[47] + (String)Protocolo[48]).toInt();
        
        if (motor_ativado) {
          if (Conectar) {
            _motor->_canal = canal;
            _motor->_Estabilizar = estabilizar;
            _motor->_Referenciar = referenciar;
            _motor->_Angulo_Offset = _offset;
            _motor->_pinoPUL = PinoModel(pul, pulB, _PWM);
            _motor->_pinoDIR = PinoModel(dir, dirB, _OUTPUT);
            _motor->_pinoENA = PinoModel(ena, enaB, _OUTPUT);
            _motor->_pinoEncoderA = PinoModel(encoderA, encAB, _INPUT, DIGITAL);
            _motor->_pinoEncoderB = PinoModel(encoderB, encBB, _INPUT, DIGITAL);
            _motor->_pinoRef000 = PinoModel(ref0, ref0B, _INPUT, DIGITAL);
            _motor->_pinoRef090 = PinoModel(ref90, ref90B, _INPUT, DIGITAL);
            _motor->_pinoRef270 = PinoModel(ref270, ref270B, _INPUT, DIGITAL);
            _motor->Inicializar();
          }
          else {
            _motor->Desligar();
          }
          vTaskDelay(500);
        }
      }
  }
  else if (_funcao == Cmd) {
      //canal;sentido;angulo
      //0;0;000.00
      String ID = ((String)Protocolo[0] + (String)Protocolo[1]);

      Motor* _motor = MotorPorID(ID);

      Sentido sentido = (Sentido)((String)Protocolo[3]).toInt();
      int angulo = ((String)Protocolo[5] + (String)Protocolo[6] + (String)Protocolo[7]).toInt();
      int porcentagem = ((String)Protocolo[9] + (String)Protocolo[10] + (String)Protocolo[11]).toInt();
      double frequencia = (porcentagem / 100.0) * FrequenciaMaxima;
      bool referenciar = ((String)Protocolo[13]) == "1";
      int _offset = ((String)Protocolo[15] + (String)Protocolo[16] + (String)Protocolo[17] + (String)Protocolo[18]).toInt();

      _frequencia = frequencia;

      _motor->_Sentido_SP = sentido;
      _motor->_Angulo_SP = angulo;
      _motor->_Angulo_Offset = _offset;

      _motor->AtualizarPWM();

      if (referenciar) {
        _motor->ResetRef(referenciar);
      }
      else {
        _motor->_MotorLigado = sentido != Parado;
      }
  }
  else if (_funcao == Tst) {
      String ID = ((String)Protocolo[0] + (String)Protocolo[1]);

      Motor* _motor = MotorPorID(ID);

      bool EmTeste = _motor->Testando;
      if (!EmTeste) {
        _motor->Testar();
      }
        
  }
}