// speedSpeedometerStepperServo.pde
// -*- mode: C++ -*-
//
// Use a Potentiometer to control the speed of several motors:
// a stepper motor with drivers with speed proportional to pot
// a stepper motor with drivers with position proportional to pot
// a servo motor porportional to pot
// a led/DC motor proportional to pot
// a led/DC motor proportional and opposite to pot
// leds/reversibleDC motor proportional to bidirectioal pot 
//

// Sim: https://wokwi.com/projects/330649945363186260
// Discussion: https://discord.com/channels/787627282663211009/787630013658824707/957769150556667954
//         and https://github.com/wokwi/wokwi-features/issues/191
// Docs:  https://docs.wokwi.com/parts/wokwi-stepper-motor
//   
// See also: https://wokwi.com/projects/327381547863769683 W/o drivers
//           https://wokwi.com/projects/327613078439985746 MultiStepper
// 
#include <WiFi.h>
#include <string.h>
#include <PubSubClient.h>
#include <AccelStepper.h> // http://www.airspayce.com/mikem/arduino/AccelStepper/

const char* ssid = "Wokwi-GUEST";
const char* password = "";
//parametros do servidor mqtt
const char* mqtt_server = "test.mosquitto.org";
const char* usuario = "";
const char* senha = "";
//Connections
const byte dirPinD = 14;  // varaible speed stepper motor
const byte stepPinD = 12;
const byte dirPinE = 2;  // varaible speed stepper motor
const byte stepPinE = 15;
const byte potPin = 34;  // Potentiometer input
int entradaE;
int entradaD;
//const bool useDeadband = true;
const int deadband = 200;
const int step_per_rev = 200;

WiFiClient espClient;
PubSubClient client(espClient);
AccelStepper stepperD(AccelStepper::DRIVER, stepPinD, dirPinD);
AccelStepper stepperE(AccelStepper::DRIVER, stepPinE, dirPinE);



float motorDSpeed = 0;
float motorDRPM = 0;

float motorESpeed = 0;
float motorERPM = 0;
float maxRPM = 50;

void setup() {
  Serial.begin(115200);
  setup_wifi();
  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);

  stepperD.setMaxSpeed(2000);
  stepperD.setSpeed(0);
  stepperE.setMaxSpeed(2000);
  stepperE.setSpeed(0);
}
bool change = false;
void loop() {
  if(!client.connected()) {
    reconnect();
  }
  client.loop();
  int pot_val = analogRead(A0);
  if (change){
    motorDRPM = map(entradaD, 0, 1023, -maxRPM, maxRPM);
    motorERPM = map(entradaE, 0, 1023, -maxRPM, maxRPM);
    change = false;

    motorDSpeed = motorDRPM * step_per_rev / 60;
    stepperD.setSpeed(motorDSpeed);
    motorESpeed = motorERPM * step_per_rev / 60;
    stepperE.setSpeed(motorESpeed);

    // analogWrite(dcPwmPin, floor(abs(potState-511.5)/2) );
    // Serial.print(potState);
    // Serial.print("->");
    // Serial.print(motorDRPM);
    // Serial.print(" ");
    Serial.println(motorESpeed);
  }

  stepperD.runSpeed();
  stepperE.runSpeed();

}
//função para encontrart a primeira ocorrencia de uma substring
//dentro de uma string maior
//retorna verdadeiro caso encontre a substring
//retorna falso caso não encontre a substring
bool findString(char* str, char* substr){
  Serial.println(strlen(str));
  for(int i = 0; i<strlen(str); i++){
    if(str[i]==substr[0]){
      Serial.println();
      bool tem = true;
      for(int k = 0; k<strlen(substr); k++){
        Serial.print(str[i+k]);
        if(str[i+k]!=substr[k]){
          tem = false;
          break;
        }
      }
      if(tem){
        Serial.println("Achou True");
        return true;
      }
      Serial.println();
    }
    
  }
  Serial.println("Achou False");
  return false;
}
//função para conectar a internet
void setup_wifi(){
  delay(10);
  //conecta na rede wifi
  Serial.println();
  Serial.print("Conectando a ");
  Serial.println(ssid);

  WiFi.begin(ssid, password);

  while(WiFi.status() != WL_CONNECTED){
    delay(500);
    Serial.print(".");
  }  
  Serial.println("");
  Serial.println("Conectado ao WiFi!");
  Serial.println("Endereco P: ");
  Serial.println(WiFi.localIP());
}
//função para reconecatar quando a conexão com o broker cair
void reconnect(){
  //loop até ser conectado
  while(!client.connected()){
    Serial.println("Tentando reconectar ao broker MQTT...");
    //tentativa de conexão
    if(client.connect("jnonsijnvsvuinsi")){
      Serial.println("Conectado");
      //INSCREVE-SE ao tópico carolina
      client.subscribe("carrinho");
      Serial.println("inscreve-se em um tópico");
    }else{
      Serial.print("Falhou, rc=");
      Serial.print(client.state());
      Serial.println("Tentar novamente em 5 segundos...");
      delay(5000);
    }
  }
}
//Quando uma mensagem chegar no tópico inscrito, essa função será chamda
void callback(const char* topic, byte* payload, unsigned int length){
  //truena/carol26jnvsvuinsi
  Serial.println((char*)payload);
  if(findString((char*)payload, "esquerda")){
    change = true;
    entradaE = 0.7*1023;
    entradaD = 1*1023;
  }else
  if(findString((char*)payload, "direita")){
    change = true;
    entradaE = 1*1023;
    entradaD = 0.7*1023;
  }else
  if(findString((char*)payload, "correr")){
    change = true;
    entradaE = 1*1023;
    entradaD = 1*1023;
  }else
  if(findString((char*)payload, "reverso")){
    change = true;
    entradaE = 0;
    entradaD = 0;
  }else
  if (findString((char*)payload, "parar")) {
    stepperD.setSpeed(0);
    stepperE.setSpeed(0);
  }
}