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

long _baudRate = 115200;

int PinoReleGeral = 23;

bool Conectado = false;

int _TaxaAmostragem;
const int PPR = 12800; // Pulsos por revolução
bool M1_Ativado = false;
bool M2_Ativado = false;
bool M3_Ativado = false;
bool M4_Ativado = false;

class Motor {
public:
  // Construtor
  Motor(String desc, int canal, int _ena, int _dir, int _pul) {
    _ID = desc;
    _canal = canal;
    _pinoENA = _ena;
    _pinoDIR = _dir;
    _pinoPUL = _pul;
  }

  // Definições
  String _ID;
  int _canal;
  int _pinoENA;
  int _pinoDIR;
  int _pinoPUL;
  
  bool Iniciado = false;
  bool Testando = false;

  // Consumo
  double Angulo;
  
  // Entrada de dados
  int _Rampa = 1000;
  double _Angulo_SP;
  Sentido _Sentido = Parado;
  bool _RampaAtivada = false;

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

    pinMode(_pinoENA, OUTPUT);
    pinMode(_pinoDIR, OUTPUT);
    pinMode(_pinoPUL, OUTPUT);

    digitalWrite(_pinoENA, HIGH);
    digitalWrite(_pinoDIR, LOW);
    digitalWrite(_pinoPUL, LOW);

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

    EnviarDadosSerial(_ID + " Iniciado");
    
    Iniciado = true;

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

  void Desligar() {
    if (!Iniciado) {
      EnviarDadosSerial(_ID + " nao esta inicializado");
      return;
    }

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

    // Desanexar o canal PWM
    ledcDetachPin(_pinoPUL);
    
    // Redefinir as configurações para os valores iniciais
    pinMode(_pinoENA, INPUT);
    pinMode(_pinoDIR, INPUT);
    pinMode(_pinoPUL, INPUT);
    
    // Outras redefinições de variáveis de estado, se necessário
    
    
    EnviarDadosSerial(_ID + " Desligado");

    Iniciado = false;

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

  void Testar() {
    Testando = true;

    Testando = false;
  }

  void Atualizar() {
    digitalWrite(_pinoENA, LOW);
    digitalWrite(_pinoDIR, _Sentido == Horario);
    digitalWrite(_pinoPUL, !digitalRead(_pinoPUL));
  }

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

  void RampaTask() {
    unsigned long previousMicrosRampa = micros();
    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;
      }
      unsigned long currentMicros = micros();
      if (currentMicros - previousMicrosRampa >= _Rampa) {
        previousMicrosRampa = currentMicros;

        if (_RampaAtivada) {
          bool Limite = false;

          Angulo++;
          /*if (Angulo >= _Angulo_SP) {
            Limite = true;
          }*/

          if (Limite) {
            _RampaAtivada = false;

            digitalWrite(_pinoENA, HIGH);
            digitalWrite(_pinoDIR, LOW);
            digitalWrite(_pinoPUL, LOW);
          }
          else {
            Atualizar();
          }
        }
        else {
          digitalWrite(_pinoENA, HIGH);
          digitalWrite(_pinoDIR, LOW);
          digitalWrite(_pinoPUL, LOW);
        }
        
      }
      
      // Aguarda metade do tempo de rampa para realizar a próxima verificação
      int wait = 10; // (_Rampa / 2.0);
      vTaskDelay(wait);
    }
  }

};

Motor M1("ET", 0, 12, 14, 27);
Motor M2("EF", 1, 13, 33, 32);
Motor M3("DT", 2, 15, 02, 04);
Motor M4("DF", 3, 05, 18, 21);

void setup() {
  Serial.begin(_baudRate);
  pinMode(PinoReleGeral, OUTPUT);
  digitalWrite(PinoReleGeral, LOW);

  TaskHandle_t SerialTaskHandle = NULL;
  xTaskCreatePinnedToCore(SerialTask, "SerialTask", 4000, NULL, 20, &SerialTaskHandle, 1);
}

void loop() {
  float x = 1509 / 300;
}

void SerialTask(void *pvParameters) {
  unsigned long previousMillisSerial = millis();
  const int TempoLeitura = 100;
  while (1)
  {
    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") {
          int _PinoReleGeral = ((String)Protocolo[5] + (String)Protocolo[6]).toInt();
          int _PPR = ((String)Protocolo[8] + (String)Protocolo[9] + (String)Protocolo[10] + (String)Protocolo[11] + (String)Protocolo[12]).toInt();

          //PPR = _PPR;
          PinoReleGeral = _PinoReleGeral;
          pinMode(PinoReleGeral, OUTPUT);
          digitalWrite(PinoReleGeral, Conectado);

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

          EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD", Conectado ? "1" : "0"));
        }
        else {
          _TaxaAmostragem = ((String)Protocolo[5] + (String)Protocolo[6] + (String)Protocolo[7] + (String)Protocolo[8] + (String)Protocolo[9]).toInt();
          bool motor_ativado = (String)Protocolo[11] == "1";

          Motor* _motor = ID == "ET" ? &M1 : ID == "EF" ? &M2 : ID == "DT" ? &M3 : &M4;
          if (motor_ativado) {
            if (Conectar) {
              _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 = ID == "ET" ? &M1 : ID == "EF" ? &M2 : ID == "DT" ? &M3 : &M4;

        _motor->_Sentido = (Sentido)((String)Protocolo[3]).toInt();
        _motor->_Angulo_SP = ((String)Protocolo[5] + (String)Protocolo[6] + (String)Protocolo[7] + (String)Protocolo[8] + (String)Protocolo[9] + (String)Protocolo[10]).toInt();
        _motor->Angulo = 0;

        _motor->_RampaAtivada = _motor->_Sentido != Parado;
      }
      else if (_funcao == Tst) {
        String ID = ((String)Protocolo[0] + (String)Protocolo[1]);

        Motor* _motor = ID == "ET" ? &M1 : ID == "EF" ? &M2 : ID == "DT" ? &M3 : &M4;

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

    vTaskDelay(pdMS_TO_TICKS(TempoLeitura));
  }
}