#include "arduinoFFT.h"
#include <avr/sleep.h>
#include <avr/interrupt.h>

// Definiciones de frecuencias
#define SAMPLE_FREQ 1000  // Frecuencia de muestreo para el ADC (Timer2)
#define SYSTEM_CLOCK_MEGAHERTZ 16
#define TIMER1_PRESCALAR 64

// Selección de onda modulada
#define ONDA_CORTA

#if defined(ONDA_CORTA)
    #define CANT_ELEM 2
    #define SUM_ANG 10000
    const int angArray[CANT_ELEM] = {5228, 4772};
#endif

#if defined(ONDA_COMPLETA)
    #define CANT_ELEM 14
    #define SUM_ANG 10000
    const int angArray[CANT_ELEM] = {2206, 348, 767, 635, 516, 815, 372, 887, 372, 827, 504, 635, 767, 348};
#endif

// Variables globales
#define samples 128
float vReal[samples];
float vImag[samples];
float vReal_Magnitud[samples];

// FFT object
ArduinoFFT<float> FFT = ArduinoFFT<float>(vReal, vImag, samples, SAMPLE_FREQ);

// Variables globales para Timer1
#define PIERNA_IZQUIERDA 0
#define PIERNA_DERECHA 1
bool piernaActiva = PIERNA_IZQUIERDA;
int numAng = 0;
int ticksArray[CANT_ELEM];

// Variables para el ADC
volatile uint16_t sampleIndex = 0;
volatile uint8_t flag_bufferLleno = 0;
volatile unsigned long lastInterruptTime = 0;
bool flag_cambioPierna = false;

// Botón para FFT
const uint8_t FFT_BUTTON_PIN = 2;  // Pin para el botón que inicia la FFT
volatile bool fftRequested = false;

void setup() {
    Serial.begin(9600);
    while (!Serial);
    Serial.println("Ready");

    pinMode(4, OUTPUT);
    pinMode(3, OUTPUT);
    pinMode(13, OUTPUT);
    pinMode(FFT_BUTTON_PIN, INPUT); // Configurar botón FFT

    // Interrupción externa para el botón que inicia la FFT
    attachInterrupt(digitalPinToInterrupt(FFT_BUTTON_PIN), requestFFT, FALLING);

    noInterrupts(); // Desactivar todas las interrupciones globales
    
    setupTimer1(50);  // Inicializar Timer1 para generar la onda modulada
    setupTimer2(SAMPLE_FREQ);  // Inicializar Timer2 para disparar el ADC

    // Configurar el ADC
    ADMUX = (1 << REFS0);  // Usar Vcc como referencia de voltaje y leer del pin A0
    ADCSRA = (1 << ADEN) | (1 << ADIE) | (1 << ADPS0) | (1 << ADPS1) | (1 << ADPS2); // Prescaler 128 (125kHz)

    interrupts(); // Habilitar todas las interrupciones globales
}

void loop() {
    if ( flag_bufferLleno == 1) {
        ADCSRA &= ~(1 << ADIE);   // Desactivar interrupciones del ADC
        TIMSK2 &= ~(1 << OCIE2A); // Desactivar interrupción de Timer2

        fftRequested = false; // Resetear el estado después de la FFT
        sampleIndex = 0;
        flag_bufferLleno = 0;
        
        procesarBuffer();  // Procesar la señal FFT

        ADCSRA |= (1 << ADIE);   // Reactivar interrupciones del ADC
        TIMSK2 |= (1 << OCIE2A); // Reactivar interrupciones de Timer2
    }
}

// Configuración de Timer1 para la onda modulada
void setupTimer1(int frec) {

    // 1. Resetear los registros de control para comenzar con la configuración desde 0
    TCCR1A = 0;                 // Reiniciar el registro TCCR1A
    TCCR1B = 0;                 // Reiniciar el registro TCCR1B
    
    // 2. Configurar el prescaler a 64 (CS12 = 0, CS11 = 1, CS10 = 1)
    TCCR1B |= B00000011;
    
    // 3. Habilitar el modo de comparación con el registro A
    TIMSK1 |= B00000010;        // Activar la interrupción por comparación en el registro A 
    
    // 4. Establecer el valor del registro de comparación A a 31250
    OCR1A = 31250;              // Configurar el valor de comparación para generar interrupciones
    
    // 5. Configuración de frecuencia (movido de SeteoFrecuencia)
    int microsTick = TIMER1_PRESCALAR / SYSTEM_CLOCK_MEGAHERTZ; // Tiempo por tick en microsegundos

    // Calcular el tiempo del semiciclo en microsegundos (para la frecuencia deseada)
    int TSemiciclo = 1000 / frec * 1000 / 2; // Tiempo del semiciclo en microsegundos

    // Calcular el total de ticks necesarios para completar un semiciclo
    int tTicksTotal = TSemiciclo / microsTick;

    // Ajustar los ticks para cada ángulo en el array ticksArray
    int ticks;
    for (int i = 0; i < CANT_ELEM; i++) {
        ticks = (int)((float)angArray[i] * (float)tTicksTotal / 10000); // Ajustar ticks según el ángulo
        ticksArray[i] = ticks; // Guardar en el array de ticks
    }
}

// Configuración de Timer2 para disparar el ADC
void setupTimer2(uint16_t freq) {
    TCCR2A = 0;
    TCCR2B = 0;
    TCNT2 = 0;
    TCCR2B |= (1 << CS22);  // Prescaler de 64 para Timer2
    OCR2A = (F_CPU / (64L * freq)) - 1;  // Ajustar la frecuencia del Timer2
    TIMSK2 |= (1 << OCIE2A);  // Habilitar interrupción por comparación en Timer2
}

// ISR de Timer1 para la onda modulada
ISR(TIMER1_COMPA_vect) {
    if(piernaActiva == PIERNA_IZQUIERDA) {
        PORTD ^= 0b00010000;  // Cambiar de estado el pin 4
    } 
    if(piernaActiva == PIERNA_DERECHA)
    {
        PORTD ^= 0b00001000;  // Cambiar de estado el pin 3
    }

    numAng++;
    if(numAng >= CANT_ELEM) {
        numAng = 0;
        PORTD &= 0b11100111;
        if(piernaActiva == PIERNA_IZQUIERDA)
        {
          flag_cambioPierna = true; //Aviso que se realizó un cambio de pierna. Así, en la ISR del ADC se cambia el pin analógico que lee el ADC
        }else
        {
          flag_cambioPierna = true
        }
               
    }

    TCNT1 = 0;                  // Reiniciar el contador del Timer 1 para la siguiente interrupción
    
    OCR1A = ticksArray[numAng];  // Actualizar el valor de comparación según el array de ticks
}

// ISR de Timer2 para disparar el ADC
ISR(TIMER2_COMPA_vect) {
    TCNT2=0;
    ADCSRA |= (1 << ADSC);  // Disparar el ADC
}

// ISR del ADC
ISR(ADC_vect) {
    if(flag_bufferLleno == 0)
    {
      if(piernaActiva == PIERNA_IZQUIERDA)
      {
        vReal[sampleIndex] = 5.0 / 1024.0 * ADC;  
        vImag[sampleIndex] = 0;
        if(flag_cambioPierna == true)
        {
          flag_cambioPierna = false;
          piernaActiva = PIERNA_DERECHA; //Se cambia aca y no en el handler del TIMER 1 para evitar un bug que hacia que la ISR del adc interpretara la ultima medicion de pierna derecha como pierna izquierda. Esto pasaba si por esa casualidad el cambio de pierna se daba en la mitad de una conversion del adc, por lo que se realizaba el cambio y luego ese valor de ADC se interpretaba como de la nueva pierna, cuando en realidad era de la anterior.
          ADMUX = (1 << REFS0) | (1 << MUX0);  // Usar Vcc como referencia de voltaje y leer del pin A1
        } 
      }else
      {
        vReal[sampleIndex] = -(5.0 / 1024.0 * ADC);  
        vImag[sampleIndex] = 0;
        if(flag_cambioPierna == true)
        {
          flag_cambioPierna = false;
          piernaActiva = PIERNA_IZQUIERDA;
          ADMUX = (1 << REFS0);  // Usar Vcc como referencia de voltaje y leer del pin A0
        }
      }

      sampleIndex++;
      if (sampleIndex >= samples)
      {
        flag_bufferLleno = 1;
      }
    }

}

// Función para procesar el buffer y hacer la FFT
void procesarBuffer() {
    Serial.println("Señal original:");
    PrintSignal(vReal, samples);

    FFT.compute(FFTDirection::Forward);
    for (int i = 0; i < samples; i++) {
        vReal_Magnitud[i] = vReal[i];
    }
    FFT.complexToMagnitude(vReal_Magnitud, vImag, samples);

    Serial.println("Frecuencia(Hz), Amplitud, Fase(rad)");
    for (uint16_t i = 0; i < (samples / 2); i++) {
        double frequency = (i * 1.0 * SAMPLE_FREQ) / samples;
        double amplitude = vReal_Magnitud[i] * 2 / samples;
        double phase = atan2(vImag[i], vReal[i]);

        Serial.print(frequency, 6);
        Serial.print(",");
        Serial.print(amplitude, 4);
        Serial.print(",");
        Serial.println(phase, 4);
    }

    FFT.compute(vReal, vImag, samples, FFTDirection::Reverse);
    Serial.println("Señal reconstruida:");
    PrintSignal(vReal, samples);
}

// Función para imprimir los valores del buffer
void PrintSignal(float *data, uint16_t bufferSize) {
    for (uint16_t i = 0; i < bufferSize; i++) {
        Serial.print(i);
        Serial.print(",");
        Serial.println(data[i]);
    }
    Serial.println();
}

// Función para manejar la solicitud de FFT
void requestFFT() {
    fftRequested = true;
}