/*ESP HAND CONTROL
Professor Rafael Oliveira - Robótica Paula Souza - mar-2023
Este é um código para ler dados de um sensor MPU6050 e enviá-los via ESP-NOW para um endereço MAC receptor */

#include <esp_now.h>
#include <WiFi.h>

//Esses são necessários para MPU
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// Variáveis de controle/status do MPU
MPU6050 mpu;
bool dmpReady = false;  // Define verdadeiro se a inicialização do DMP for bem sucedida
uint8_t devStatus;      // Retorna o status após cada operação do dispositivo (0 = sucesso, !0 = erro)
uint8_t fifoBuffer[64]; // Buffer de armazenamento FIFO
Quaternion q;           // [w, x, y, z]         contêiner de quatérnio
VectorFloat gravity;    // [x, y, z]            vetor de gravidade
float ypr[3];           // [yaw, pitch, roll]   contêiner de yaw/pitch/roll e vetor de gravidade

// Endereço MAC receptor
uint8_t receiverMacAddress[] = {0xE8,0xDB,0x84,0x0F,0xAD,0xB4};  //E8:DB:84:0F:AD:B4

// Estrutura de dados da embalagem
struct PacketData 
{
  byte xAxisValue;
  byte yAxisValue;
  byte zAxisValue;  
};
PacketData data;

// callback quando os dados são enviados
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status)
{
  //Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Mensagem enviada" : "Falha ao enviar mensagem");
}

void setupMPU()
{
  // Junte o barramento I2C (a biblioteca I2Cdev não faz isso automaticamente)
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      Wire.setClock(400000); // 400kHz I2C clock. Comente esta linha se tiver dificuldades de compilação
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif

  mpu.initialize();
  devStatus = mpu.dmpInitialize();
  // Verifique se funcionou (retorna 0 se sim)
  if (devStatus == 0) 
  {
      // Tempo de calibração: gere deslocamentos e calibre nosso MPU6050
      mpu.CalibrateAccel(6);
      mpu.CalibrateGyro(6);
      mpu.setDMPEnabled(true);
      dmpReady = true;
  } 
}

// Inicia a comunicação serial com uma taxa de transmissão de 115200 baud
void setup()
{
Serial.begin(115200);

// Define o modo WiFi como estação (station)
WiFi.mode(WIFI_STA);

// Inicializa o ESP-NOW e verifica se houve algum erro
if (esp_now_init() != ESP_OK)
{
Serial.println("Erro ao inicializar o ESP-NOW");
return;
}
else
{
Serial.println("Sucesso: ESP-NOW inicializado");
}

// Registra a função de callback para quando os dados forem enviados
esp_now_register_send_cb(OnDataSent);

// Cria a estrutura que armazenará as informações do receptor (peer)
esp_now_peer_info_t peerInfo;

// Copia o endereço MAC do receptor para a estrutura
memcpy(peerInfo.peer_addr, receiverMacAddress, 6);

// Define o canal de comunicação como 0
peerInfo.channel = 0;

// Define que a criptografia não será utilizada
peerInfo.encrypt = false;

// Adiciona o receptor como peer
if (esp_now_add_peer(&peerInfo) != ESP_OK)
{
Serial.println("Falha ao adicionar o receptor (peer)");
return;
}
else
{
Serial.println("Sucesso: Receptor (peer) adicionado");
}

// Configura o sensor MPU6050
setupMPU();
}

void loop()
{
// Verifica se o sensor está pronto para ser usado
if (!dmpReady) return;

// Lê um pacote de dados do FIFO
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))
{
// Lê as informações de rotação do sensor
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

// Converte os valores para inteiros e os limita entre -90 e 90 graus
int xAxisValue = constrain(ypr[2] * 180/M_PI, -90, 90);
int yAxisValue = constrain(ypr[1] * 180/M_PI, -90, 90);
int zAxisValue = constrain(ypr[0] * 180/M_PI, -90, 90); 

// Mapeia os valores entre 0 e 254 e os armazena na estrutura de dados
data.xAxisValue = map(xAxisValue, -90, 90, 0, 254); 
data.yAxisValue = map(yAxisValue, -90, 90, 0, 254);
data.zAxisValue = map(zAxisValue, -90, 90, 0, 254);    

// Envia os dados para o receptor (peer) e verifica se houve algum erro
esp_err_t result = esp_now_send(receiverMacAddress, (uint8_t *) &data, sizeof(data));

// Cria uma string com os valores lidos do sensor e a imprime no monitor serial
String inputData  = inputData + "values " + xAxisValue + "  " + yAxisValue + "  " + zAxisValue;
Serial.println(inputData);

// Tempo aguardado antes de continuar a execução
delay(50);            
  }
}
