/* --------------------------------------------------------------------------
  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 "kiss_fftr.h"

// threshold  in milli G (gravity)
#define threshold 0.0

// FFT stuff  500samples/sec take 512 data points

#define FSAMP 500
#define NSAMP 512

// data to record mpu6050 data
// spectrum output to send via UDP
// N.B. spectrum[0] is the max idx instead of the sum of all
unsigned short spectrum[NSAMP/2];
// FFT DECLARATION

volatile int current_in=1;
kiss_fft_scalar fft_in1[NSAMP]; // kiss_fft_scalar is a float
kiss_fft_scalar fft_in2[NSAMP]; // kiss_fft_scalar is a float
kiss_fft_cpx fft_out[NSAMP];
kiss_fftr_cfg cfg;
float max_power = 0;
int max_idx = 0;
/**
 * 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 */ 

// DO_FFT this is the thread to read MPU6050 on second cpu and send UDP
void Do_FFT()
{
   

    if(current_in==1)
    kiss_fftr(cfg , fft_in1, fft_out);
    else
    kiss_fftr(cfg , fft_in2, fft_out);
    // find max freq
    // compute power and calculate max freq component
    max_power = 0;
    max_idx = 0;
    // any frequency bin over NSAMP/2 is aliased (nyquist sampling theorum)
    for (int i = 1; i < NSAMP/2; i++) {
        float power = sqrt(fft_out[i].r*fft_out[i].r+fft_out[i].i*fft_out[i].i);
        spectrum[i]= (unsigned short) power;
        if (power>max_power) {
          max_power=power;
          max_idx = i;
        }
      }
    if(max_power > threshold)
    {
      spectrum[0]=max_idx;
      //Serial.printf("G:%.03f\n\0", max_power);
      for(int i = 1; i < sizeof(unsigned short) * (NSAMP/2); ++i) {
        Serial.printf("%d", spectrum[i]);
      }
      //int ln = strlen(UDP_buffer);
      //SendUDP(SEND_TO_IP,SEND_TO_PORT,spectrum,sizeof(unsigned short) *  (NSAMP/2));
    }
}
/**
 * tasks;
 */
void task_mpu(void * pvParameter)
{
    char str[512];
    int recordIdx=0;
    /**
    * 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 ...");

    // allocate FFT
    cfg = kiss_fftr_alloc(NSAMP, false, 0, 0);


   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
            );
    //Serial.println(str);
    float g[4];
    g[0] = (float) s1_mpu6050.accX;
    g[1] = (float) s1_mpu6050.accY;
    g[2] = (float) s1_mpu6050.accZ;
    // convert  mpu6050 digital value to milli g  (gravity)
    float gFactor = 2000.0 / 32767.0;
    g[3] = sqrt(g[0]*g[0]+ g[1]*g[1] + g[2]*g[2]) * gFactor;

    if(current_in == 1){fft_in1[recordIdx] = g[3];} 
      else {fft_in2[recordIdx] = g[3];}
    recordIdx++;
    // did we fill completely the record
    if(recordIdx>=NSAMP)
    {

      if(current_in == 1)
      {
        // copy second half to next fft_in1
          memcpy(fft_in2,&fft_in1[NSAMP/2],sizeof(float)*NSAMP/2);
      }
      else
      {
          memcpy(fft_in1,&fft_in2[NSAMP/2],sizeof(float)*NSAMP/2);
      }

      Do_FFT();


      // prepare to record next dat on alternate record
      recordIdx=NSAMP/2;
      current_in = (current_in) == 1 ? 2 :1;
    }

	  //vTaskDelay(1/portTICK_PERIOD_MS);
  }
	kiss_fft_free(cfg);
}

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);
}