// Dispositivo: Direcional
// Versão Firmware: 4
// Ultima atualização: 28/11/2023

#include "C:\Zendion Inc\agrobot_base\Firmware\Modulos\SerialService.h"
#define D_Code Dir

#define _pinoLED RGB_BUILTIN
int _pinoReleGeral = -1;

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;



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

  // Definições
  String _ID;
  int _canal;
  int _pinoENA;
  int _pinoDIR;
  int _pinoPUL;
  int _pinoEncoderA;
  int _pinoEncoderB;
  int _pinoRef000;
  int _pinoRef090;
  int _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 > -1) {
      AtualizarPWM();
    }
    if (_pinoENA > -1) {
      pinMode(_pinoENA, OUTPUT);
      digitalWrite(_pinoENA, HIGH);
    }
    if (_pinoDIR > -1) {
      pinMode(_pinoDIR, OUTPUT);
      digitalWrite(_pinoDIR, LOW);
    }
    if (_pinoEncoderA > -1) {
      pinMode(_pinoEncoderA, INPUT);
      ulEncoderA = digitalRead(_pinoEncoderA);
      EA_M = false;
    }
    if (_pinoEncoderB > -1) {
      pinMode(_pinoEncoderB, INPUT);
      ulEncoderB = digitalRead(_pinoEncoderB);
      EB_M = false;
    }
    if (_pinoRef000 > -1) {
      pinMode(_pinoRef000, INPUT);
      Ag0 = digitalRead(_pinoRef000) == 1;
      _Ag0 = digitalRead(_pinoRef000) == 1;
    }
    if (_pinoRef090 > -1) {
      pinMode(_pinoRef090, INPUT);
      Ag90 = digitalRead(_pinoRef090) == 1;
      _Ag90 = digitalRead(_pinoRef090) == 1;
    }
    if (_pinoRef270 > -1) {
      pinMode(_pinoRef270, INPUT);
      Ag270 = digitalRead(_pinoRef270) == 1;
      _Ag270 = digitalRead(_pinoRef270) == 1;
    }
    

    xTaskCreatePinnedToCore(&Motor::RampaTaskWrapper, "RampaTask", 5000, this, 25 - _canal, &RampaTaskHandle, tskNO_AFFINITY);


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

    ResetRef(_Referenciar);

    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, _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
    if (_pinoENA > -1) {
      pinMode(_pinoENA, INPUT);
    }
    if (_pinoDIR > -1) {
      pinMode(_pinoDIR, INPUT);
    }
    if (_pinoPUL > -1) {
      ledcDetachPin(_pinoPUL);
      pinMode(_pinoPUL, INPUT);
    }
    if (_pinoEncoderA > -1) {
      pinMode(_pinoEncoderA, INPUT);
    }
    if (_pinoEncoderB > -1) {
      pinMode(_pinoEncoderB, INPUT);
    }
    if (_pinoRef000 > -1) {
      pinMode(_pinoRef000, INPUT);
    }
    if (_pinoRef090 > -1) {
      pinMode(_pinoRef090, INPUT);
    }
    if (_pinoRef270 > -1) {
      pinMode(_pinoRef270, INPUT);
    }
    
    
    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();

      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() {
    if (_pinoENA > -1) {
      digitalWrite(_pinoENA, !_MotorLigado);
    }
    if (_pinoDIR > -1) {
      digitalWrite(_pinoDIR, _Sentido_SP != Horario);
    }
  }

  void AferirPosicao() {
    if (_pinoEncoderA > -1 && _pinoEncoderB > -1) {
      int leituraEncoderA = digitalRead(_pinoEncoderA);
      int leituraEncoderB = digitalRead(_pinoEncoderB);

      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() {
    if (_pinoRef000 > -1) {
        _Ag0 = digitalRead(_pinoRef000) == 1;
    }
    if (_pinoRef090 > -1) {
        _Ag90 = digitalRead(_pinoRef090) == 1;
    }
    if (_pinoRef270 > -1) {
        _Ag270 = digitalRead(_pinoRef270) == 1;
    }

    if (GrausPino == 0) {
        if (_pinoRef000 > -1) {
          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 > -1) {
          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 > -1) {
          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 > -1) {
        Ag0 = _Ag0;
    }
    if (_pinoRef090 > -1) {
        Ag90 = _Ag90;
    }
    if (_pinoRef270 > -1) {
        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);

  if (_pinoReleGeral > -1) {
    pinMode(_pinoReleGeral, OUTPUT);
    digitalWrite(_pinoReleGeral, LOW);
  }
}

void loop() {
  if (Serial.available() > 3) {
    String Protocolo = "";
    F_Code _funcao = Nda;
    while (Serial.available()) {
      char Entrada = (char)Serial.read();
      if (Entrada == EndLine) {
        break;
      }
      Protocolo += Entrada;
      if (Protocolo.length() == 3) {
        if (_funcao == Nda) {
          _funcao = (F_Code)((String)Protocolo[0] + (String)Protocolo[1] + (String)Protocolo[2]).toInt();
          Protocolo = "";
        }
      }
    }

    EnviarDadosSerial("OK");

    if (_funcao == Chk) {
      EnviarDadosSerial(MontarProtocoloVerificacao(D_Code));
    }
    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();
        int pinoReleGeral = ((String)Protocolo[11] + (String)Protocolo[12]).toInt();
        AA_Ref000 = (String)Protocolo[14] == "1";
        AA_Ref090 = (String)Protocolo[16] == "1";
        AA_Ref270 = (String)Protocolo[18] == "1";

        if (pinoReleGeral > -1) {
          pinMode(_pinoReleGeral, INPUT);
          _pinoReleGeral = pinoReleGeral;
          pinMode(_pinoReleGeral, OUTPUT);
          digitalWrite(_pinoReleGeral, Conectar);
        }

        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 pul = ((String)Protocolo[13] + (String)Protocolo[14]).toInt();
        int dir = ((String)Protocolo[16] + (String)Protocolo[17]).toInt();
        int ena = ((String)Protocolo[19] + (String)Protocolo[20]).toInt();
        int encoderA = ((String)Protocolo[22] + (String)Protocolo[23]).toInt();
        int encoderB = ((String)Protocolo[25] + (String)Protocolo[26]).toInt();
        int ref0 = ((String)Protocolo[28] + (String)Protocolo[29]).toInt();
        int ref90 = ((String)Protocolo[31] + (String)Protocolo[32]).toInt();
        int ref270 = ((String)Protocolo[34] + (String)Protocolo[35]).toInt();
        int _offset = ((String)Protocolo[37] + (String)Protocolo[38] + (String)Protocolo[39] + (String)Protocolo[40]).toInt();

        if (motor_ativado) {
          if (Conectar) {
            _motor->_canal = canal;
            _motor->_Estabilizar = estabilizar;
            _motor->_Referenciar = referenciar;
            _motor->_Angulo_Offset = _offset;
            _motor->_pinoPUL = pul;
            _motor->_pinoDIR = dir;
            _motor->_pinoENA = ena;
            _motor->_pinoEncoderA = encoderA;
            _motor->_pinoEncoderB = encoderB;
            _motor->_pinoRef000 = ref0;
            _motor->_pinoRef090 = ref90;
            _motor->_pinoRef270 = ref270;
            _motor->Inicializar();
          }
          else {
            _motor->Desligar();
          }
          vTaskDelay(pdMS_TO_TICKS(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();
      }
        
    }

  }
}