#include <Arduino_FreeRTOS.h>
#include <queue.h>
#include <arduinoFFT.h>
#include <time.h>
#define N_SAMPLES 64
#define E 1517
#define C 1911
#define G 1276
#define g 2551
#define R 0
// Define the melody for the Super Mario theme
int melody[] = {E, R, E, R, R, E, R, R, C, R, E, R, R, G, R, R, R, R, R, g, R};
int val = sizeof(melody) / sizeof(melody[0]);

TaskHandle_t  RT4; 
TaskHandle_t  RT3;  



const uint16_t samples = 64;

  
  const double samplingFrequency = 5000; 
  double vReal[samples];
  double vImag[samples];
  QueueHandle_t queue1 = xQueueCreate(1, sizeof(double) * samples);
  QueueHandle_t queue2 = xQueueCreate(1, sizeof(TickType_t));


 
void setup() {
  // put your setup code here, to run once:

  while(!Serial){

  }
  //RT3p0(); 
  Serial.begin(9600);
  pinMode(24,OUTPUT);
  digitalWrite(24,LOW);
  DDRL |= (1 << PORTL2);  // 47
    // initialize timer A, B, C to zero
    TCCR4A = 0;
    TCCR4B = 0;
    TCCR4C = 0;

    TCCR4A |= (1 << COM4A0);  // enable compare output mode of TCCR4A
    TCCR4B |= (1 << CS41);    // set precale to 8
    TCCR4B |= (1 << WGM42);   // enable Waveform Generation Mode to WGM42

    TCNT4 = 0;  // conunt from 0

    // set output
    DDRH |= 1 << DDH3;
    PORTH |= 1 << DDH3;


  xTaskCreate(task1Function, "Task1", 128, NULL, 1, NULL);
  xTaskCreate(task2Function, "Task2", 128, NULL, 1, NULL);
  xTaskCreate(RT_3, "RT_3", 128, NULL, 0, &RT3);
  

}

void loop() {
  // put your main code here, to run repeatedly:

}

void task1Function(void *pvParameters){
  for(;;){
    digitalWrite(24, !digitalRead(24));
    vTaskDelay(pdMS_TO_TICKS(100));
    digitalWrite(24, !digitalRead(24));
    vTaskDelay(pdMS_TO_TICKS(200));
  }
}

void task2Function(){
  int i =0; 
  int count = 0; 
  for(;;){
    i = (i == val) ? 0:i +1; 
    OCR4A = melody[i];
    vTaskDelay(pdMS_TO_TICKS(100));

    if(i==val){
     vTaskDelay(pdMS_TO_TICKS(1500));
     count++; 
    }
    if(count ==3){
      vTaskDelete(NULL);
    }
  }
  
}

void RT_3(void *pvparameter){
  srand((unsigned int) time (NULL));
  // for(int i =0; i<samples; i++){
  //   int randInt = rand(); 
  //   double randomValue = (double) randInt/ RAND_MAX * 100;
  //   vReal[i] = randomValue;
  //   Serial.println(vReal[i],DEC);
    
  // }
  // for(int i = 0; i<samples; i++){
  //   vImag[i]= 0;
  //  // Serial.println(vImag[i],DEC);
  // }
  double data[samples];
  TickType_t totalTime =0, cTime= 0;

  for(int i = 0; i<samples; i++){
    data[i]= (double)rand()/ RAND_MAX * (100-0) + 100; 
    Serial.println(data[i]);
  }

  xTaskCreate(RT_4, "RT_4", 256, NULL,1 ,&RT4); 
  //xQueueSend(queue1, &data, pdMS_TO_TICKS(100));
  vTaskSuspend(NULL);

  for(int i = 0; i <5; i++){
    if(xQueueSend(queue1, &data, pdMS_TO_TICKS(100)) == pdPASS){
      Serial.println("Data Sent succesfully");
      vTaskResume(RT4);
      vTaskSuspend(RT3);
    }else{
      Serial.println("failed");
    }
    if(xQueueReceive(queue2, &cTime, portMAX_DELAY) == pdPASS){
      totalTime = totalTime + cTime; 
      Serial.println("recieved compution");
    }else{
      Serial.println("not recieved compution");
    }
  }
  Serial.println(totalTime * 1000/ configTICK_RATE_HZ);
  
}

void RT_4(void *pvparameter){
  TickType_t startTime, endTime, compTime;
  double data[samples];
  vImag[samples] = {0};
  TickType_t cTime;

  if(xQueueReceive(queue1, &data, pdMS_TO_TICKS(100)) == pdPASS){
  startTime = xTaskGetTickCount(); 
  arduinoFFT FFT = arduinoFFT(vReal, vImag, samples, samplingFrequency); 
  FFT.Windowing(FFT_WIN_TYP_HAMMING, FFT_FORWARD);
  FFT.Compute(FFT_FORWARD);

  endTime = xTaskGetTickCount();
  compTime = endTime - startTime; 
  xQueueSend(queue2, &compTime, 0);
  }
  
  if (xQueueSend(queue2, &cTime, portMAX_DELAY) == pdPASS ){
  Serial.println(compTime * 1000 / configTICK_RATE_HZ);
      vTaskResume(RT3);
      vTaskSuspend(RT4);
  //resumert3
  //supsepdnRT4
  }

}