/* --------------------------------------------------------------------------
  Baseada na biblioteca MPU6050_light e no exemplo I2C Espressif;
  Modificado por Prof. Fernando Simplicio;
 * --------------------------------------------------------------------------
   This example code is in the Public Domain (or CC0 licensed, at your option.)
   Unless required by applicable law or agreed to in writing, this
   software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
   CONDITIONS OF ANY KIND, either express or implied.
*/

/**
 * C library
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

/**
 * FreeRTOS
 */
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"

/**
 * ESP32;
 */
#include "esp_err.h"
#include "esp_log.h"
#include "driver/i2c.h"

/**
 * lib mpu;
 */
#include "mpui2c.h"
#include "mpu6050.h"
#include "qmqtt.h"

/**
 * Config. I2C;
 */
#define I2C_MASTER_SCL_IO 5           /*!< I2C pin clock */     
#define I2C_MASTER_SDA_IO 4           /*!< I2C pin data */    
#define I2C_MASTER_PORT I2C_NUM_0     /*!< I2C master port */ 

/**
 * Função responsável pela recepção das mensagens via MQTT;
 */
esp_err_t event_handler (mqtt_event_t * event)
{
	char str[100] = {0,}; 
	snprintf(str, sizeof(str)-1, "Topico: %.*s, Message: %.*s\n", 
									event->topic_size, 
									event->topic_name, 
									event->message_size, 
									event->message);
	Serial.println(str);
  return ESP_OK;
}

/**
 * tasks;
 */
void task_mpu(void * pvParameter)
{
    char str[512];

    /**
    * inicializa i2c;
    */
    if(i2c_master_init(I2C_MASTER_PORT, I2C_MASTER_SCL_IO, I2C_MASTER_SDA_IO)== ESP_OK) {
      Serial.println("Master I2C inicializado ...");
    }
    /**
    * inicializa acelerômetro MPU6050-1;
    */
    mpu6050_t s1_mpu6050 = {
      .mpui2cPort = I2C_MASTER_PORT, 
      .mpuAddress = 0x68,
      .gyroXoffset = 0,
      .gyroYoffset = 0,
      .gyroZoffset = 0,
    };
    mpu6050_init(&s1_mpu6050);
    Serial.println("S1-MPU6050 Inicializado ...");
    /**
    * inicializa wifi e conexão com o broker MQTT;
    */
    if(mqtt_connect(&event_handler) == ESP_OK) {
      Serial.println("WiFi conectado e assinatura realizada com sucesso...");
    }

   for(;;)
   {
    /**
    * Leitura do MPU6050-1;
    */
    mpu6050_update(&s1_mpu6050);
    snprintf(str, sizeof(str)-1, "temp: %.2f, \nAccX: %.2f, AccY: %.2f, AccZ: %.2f, \nGyroX: %.2f, GyroY: %.2f, GyroZ: %.2f\n", 
                  s1_mpu6050.temp,  //temperatura
                  /* accel */
                  s1_mpu6050.accX, 
                  s1_mpu6050.accY, 
                  s1_mpu6050.accZ, 
                  /* giro */
                  s1_mpu6050.gyroX, 
                  s1_mpu6050.gyroY, 
                  s1_mpu6050.gyroZ
            );

	 /**
		* Publica tópico; 
		*/		
		if(mqtt_topic_publish(str,          //Endereço do vetor que contém os bytes a serem publicados;
													strlen(str),  //Quantidade de bytes a serem publicados
													MQTT_PUBLISH_TOPIC, //Tópico de publicação
													0, 						//QoS-0 (0, 1 ou 2)	
													0             //Sem retenção da mensagem pelo broker;
													) == ESP_OK) {
          Serial.printf("mensagem publicada com sucesso...\n %s", str); 
    }

	  vTaskDelay(1000/portTICK_PERIOD_MS);
  }
	
}

void setup (void) 
{
  Serial.begin(115200);

	if(xTaskCreate(task_mpu, "task_mpu", 1024 * 10, NULL, 2, NULL) != pdPASS) {
    Serial.println("error - Nao foi possivel alocar task_mpu.\r\n" );  
    return;   
  }
 
}

void loop(void) {
  vTaskDelay(5000/portTICK_PERIOD_MS);
}