//Bibliotecas
#include "Adafruit_MPU6050.h"
#include "Adafruit_Sensor.h"
#include "Wire.h"
#include "FS.h"
#include "SD.h"
#include "SPI.h"
#include "HX711.h"
//Macros
#define CELDA_DT 12
#define CELDA_CLK 13
#define CS_PIN 5
#define CD_PIN 15
#define LED_PIN 32
#define BOT_PIN 34
#define NOM_ARCH "/datos"
#define NOM_EXT ".csv"
#define MU_MAX 2500 //En mi laptop, esto es suficiente para los 2 minutos de simulación

//Variables/estructuras globales
File sdArch;
HX711 cDC;
Adafruit_MPU6050 ace;
String nomArch;
float tIni;
float *fEmpuje;
float *acel;
float *tiempo;
//Prototípos de funciones
void sdInic();
void nArch();
void ccInic();
void espBot();
void acInic();
int muestreo();
void ajusteTiempo(int muestras, float *tiempo, float tInicial);
void escCSV(float *tiempo, float *fEmpuje, float *acel, int muestras);
float valAcel(float acelReal, int i);
float valPeso(float pesoReal, int i);

void setup() {
   //Configuración de la comunicación serial
  Serial.begin(9600);
  Serial.println("Inicio del programa");
  //Configuración de pines
  pinMode(CD_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);
  pinMode(BOT_PIN, INPUT);
  digitalWrite(LED_PIN, HIGH); // Empieza el LED apagado.
  //Inicialización de lector-escritor SD
  Serial.println("Inicializando lector-escritor SD");
  sdInic();
  //Creación de archivo
  Serial.println("Creando archivo");
  nArch();
  //Inicializa celda de carga
  Serial.println("Inicializando celda de carga");
  ccInic();
  //Inicializa acelerómetro
  Serial.println("Inicializando el acelerómetro");
  acInic();
  Serial.println("Listo para medir datos");
  fEmpuje = new float[MU_MAX];
  acel = new float[MU_MAX];
  tiempo = new float[MU_MAX]; //Número máximo de muestras. Ya que no hay arreglos dinámicos en C, se necesita por seguridad.
  int nMuestras = muestreo();
  ajusteTiempo(nMuestras, tiempo, tIni); //Se efectua el muestreo y se ajustan los valores de tiempo
  escCSV(tiempo, fEmpuje, acel, nMuestras); //Se escriben los datos al archivo
  Serial.println("EXITO");
}

void loop() {
  delay(100);
}

void sdInic(){
  if (!SD.begin(CS_PIN)) {
    Serial.println("Error de inicialización");
    while(1);
  }
  Serial.println("Inicialización exitosa");
}

void nArch() {
  int numArch = 0;
  nomArch = String(NOM_ARCH) + String(NOM_EXT);
  //Evita sobrescribir archivos
  while (SD.exists(nomArch)) {
    nomArch = NOM_ARCH + String(numArch);
    nomArch += NOM_EXT;
    numArch++;
  }
  // Crea el archivo
  sdArch = SD.open(nomArch, FILE_WRITE);
  
  // Revisa que el código haya sido creado correctamente
  if (sdArch) {
    Serial.print("Archivo creado: ");
    Serial.println(nomArch);
    sdArch.close();  // Cierra el archivo
  } else {
    Serial.println("Error creando archivo");
    while (1);
  }
}

void ccInic() {
  if (!digitalRead(CD_PIN)){
    Serial.println("Error, no se detecta ninguna tarjeta");
    while (1);
  }
  cDC.begin(CELDA_DT, CELDA_CLK);
  if (cDC.is_ready()){
    cDC.set_scale();
    Serial.println("Celda inicializada");
    Serial.println(("Calibrando celda..."));
    cDC.tare();
    //Se calibra con una pesa de 1Kg
    digitalWrite(LED_PIN, LOW); //Prende el LED, el usuario coloca la pesa en la celda de carga
    Serial.println("Apriete el botón");
    espBot();
    long med = cDC.get_units(10);
    long valCal = med / 9.8067; //Calbración a Newtons, se utiliza el valor arrojado por el acelerómetro para 1G, simplemente por consistencia.
    cDC.set_scale(valCal);
    digitalWrite(LED_PIN, HIGH); // Apaga el LED
    Serial.print("Valor de calibración:     ");
    Serial.println(float(valCal));
    Serial.println("Calibración exitosa");
  }
  else {
    Serial.println("Error");
    while (1);
  }
}

void espBot(){ // Esta función espera a que el botón sea apretado
  while (1){
    while(!digitalRead(BOT_PIN)){
      delay(10); //Debouncing
      if(!digitalRead(BOT_PIN)){
        return;
      }
    }
  }
}

void acInic(){
  if (!ace.begin()){
    Serial.println("Error");
    while (1);
  }
  Serial.println("Acelerómetro inicializado con exito");
}

int muestreo(){
  digitalWrite(LED_PIN, LOW); //Prende LED
  Serial.println("Apriete el botón");
  espBot(); //Espera al botón para empezar a tomar datos
  digitalWrite(LED_PIN, HIGH);  //Apaga LED;
  sensors_event_t acc, gyr, temp;
  int i = 0;
  delay(2000); //Espera 2 segundos para evitar que el botón de inicio finalice el muestreo
  tIni = millis(); //Almacena el tiempo de inicio de la toma de datos
  while(1){
    if(!digitalRead(BOT_PIN) | i == MU_MAX){
      delay(10); // debouncing
      if(!digitalRead(BOT_PIN) | i == MU_MAX){
        Serial.print("Muestreo concluido:     ");
        Serial.print(i);
        Serial.println(" muestras");
        return i; //Detiene el muestreo al apretar el botón
      }
    }
    tiempo[i] = millis(); //No se resta el tiempo inicial para no retrasar al muestreo
    fEmpuje[i] = valPeso(cDC.get_units(1), i); //No se toma promedio para no retrasar al muestreo. GENERA DATOS ALEATORIOS
    ace.getEvent(&acc, &gyr, &temp); //Se actualiza el valor del acelerómetro.
    acel[i] = valAcel(acc.acceleration.z, i); //Asumiendo que en un banco de pruebas la aceleración es unidimensional
    i++;
  }
}

void ajusteTiempo(int muestras, float *tiempo, float tInicial){ // Esta función ajusta un vector de tiempo a un punto de referencia
  for (int i = 0; i < muestras; i++){
    tiempo[i] -= tInicial;
    tiempo[i] = tiempo[i] / 1000; //Milis a segundos
  }
  Serial.println("Vector de tiempo ajustado");
  return;
}

void escCSV(float *tiempo, float *fEmpuje, float *acel, int muestras) {
    sdArch = SD.open(nomArch, FILE_WRITE);
    if (sdArch) { // Verificando que sí exista el archivo
        sdArch.println("Tiempo,Empuje,Aceleracion"); //Escribe encabezado
        Serial.println("Tiempo,Empuje,Aceleracion");
        for (int i = 0; i < muestras; i++) {
            sdArch.print(tiempo[i], 4); //4 decimales
            Serial.print(tiempo[i], 4);
            sdArch.print(","); // Separador de columna
            Serial.print(",");
            sdArch.print(fEmpuje[i], 4);
            Serial.print(fEmpuje[i], 4);
            sdArch.print(",");
            Serial.print(",");
            sdArch.println(acel[i], 4);
            Serial.println(acel[i], 4); //Termina la linea
        }
        sdArch.close(); // Cierra el archivo
        Serial.println("Datos escritos");
    } else {
        Serial.println("Error");
        while (1);
    }
}

//A PARTIR DE AQUÍ, EL CÓDIGO ES PARA INTRODUCIR ARTIFICIALMENTE UNA VARIACIÖN EN LOS VALORES MEDIDOS
float valAcel(float acelReal, int i) {
   return acelReal + (i*0.05)*(i*0.05)*random (1.2,1.3) - i*random(1, 1.1);
}

float valPeso(float pesoReal, int i){
  return pesoReal - (0.0001*i*random(1, 1.1));
}