#include <SDL2/SDL_image.h>
#include <SDL2/SDL_timer.h>
#include <SDL2/SDL_ttf.h>
#include <stdio.h>
#include <cmath>
#include <unistd.h>
#include <iostream>
#include <chrono>
#include "scope.h"

// Functions to try to process Constant Q transform
piano_process_class::piano_process_class()
  {
  }

piano_process_class::~piano_process_class()
  {
  }

double piano_process_class::goertzel(int num_samples, int target_frequency, int sample_rate, 
  Sint16 *input_buffer)
  {
  double dinput_buffer;
  double magnatude;
  int k, i;
  double doublenumsamples;
  double omega, sine, cosine, coeff, q0, q1, q2, real, imag;
  double scalingfactor; 
 
  // Initialize constant stuff

  doublenumsamples = (double)num_samples;
  scalingfactor = doublenumsamples / 2.0;
  k = (int)(0.5 + ((doublenumsamples * (double)(target_frequency)) / (double)sample_rate));
  omega = (2.0 * M_PI * (double)k) / doublenumsamples;
  sine = sin(omega);
  cosine= cos(omega);
  coeff = 2.0 * cosine;

  // Init q's
  q0 = 0;
  q1 = 0;
  q2 = 0;


  // First stage (2nd order IIR filter)
  for (i = 0; i < num_samples; i += 1)
    {
    dinput_buffer = (double)*(input_buffer + i);
    q2 = q1;
    q1 = q0;
    q0 = coeff * q1 - q2 + dinput_buffer;
//    printf("q0 %f; dinput_buffer %f: integer_input_buffer %d\n", q0, dinput_buffer, *(input_buffer + i));
    }

  // Seconod stage (FIR filter)
  real = (q0 - q1 *cosine) / scalingfactor;
  imag = (-q1 * sine) / scalingfactor;
  magnatude = sqrt((real * real) + (imag * imag));
  

  // Test Prints

  // printf("doublesamples %f; scalingfactor %f; k %d\n", doublenumsamples, scalingfactor, k);
  // printf("omega %f; sine %f; cosine %f; coeff %f\n", omega, sine, cosine, coeff);
  // printf("magnatude %f\n", magnatude);

  // printf("goertzel done\n");
  return magnatude;
  }

struct piano_data_struct *piano_process_class::process_piano_data(Sint16 *input_buffer)
  {
  int bucket_counter;
  bucket_data_struct *current_bucket;
  FILE *result_plot_file;
  FILE *input_waveform_file;
  FILE *note_result_plot_file;
  Sint16 *current_read_buffer_in;
  Sint16 *current_read_buffer_out;
  int print_file_counter;
  int count_for_transfering_buffers;
  struct piano_note_struct *working_note;
  struct piano_note_struct *strongest_note;
  int note_counter;


  // Before doing anything, we need to 1) update the full size buffer
  // and ensure that it is full before processing,
  // Otherwize return NULL if not.

  if (main_buffer_read_count < FFT_NUMBER_READ_BUFFERS)
    {
    current_read_buffer_in = input_buffer;   
    current_read_buffer_out = full_size_buffer + READ_BUFFER_SIZE * main_buffer_read_count;
    for (count_for_transfering_buffers = 0; count_for_transfering_buffers < READ_BUFFER_SIZE;
         count_for_transfering_buffers += 1)
      {
      *current_read_buffer_out = *current_read_buffer_in;
      current_read_buffer_out += 1;
      current_read_buffer_in += 1;
      }
      
    main_buffer_read_count += 1;
    return NULL;
    }
  else
    {
    main_buffer_read_count = 0;
    }
 
  input_waveform_file = fopen("waveform_of_input","w");
  if (input_waveform_file == NULL)
    {
    printf("cant open input waveform_file\n");
    exit (0);
    }

  for (print_file_counter = 0; print_file_counter < READ_BUFFER_SIZE * FFT_NUMBER_READ_BUFFERS;
      print_file_counter += 2)
   {
   fprintf(input_waveform_file, "%d      %d\n", print_file_counter, *(full_size_buffer + print_file_counter));
   // printf("%d      %f\n", print_file_counter, (double)*(full_size_buffer + print_file_counter));
   }

  fclose (input_waveform_file);
  
  // Go through all of the buckets to get their strenght using Goertzel
  // Test plotfile
  result_plot_file = fopen("result_data_for_gnuplot", "w");
  if (result_plot_file == NULL)
    {
    printf("oops cant open result file\n");
    perror("cant  open file\n");
    exit(0);
    }

  note_result_plot_file = fopen("note_data_for_gnuplot", "w");
  if (note_result_plot_file == NULL)
    {
    printf("oops cant open note result file\n");
    perror("cant  open file\n");
    exit(0);
    }

  //clear the strengh in all notes
  working_note = my_piano_notes;
  for (note_counter = 0; note_counter < TOTAL_NOTES; note_counter += 1)
    {
    (working_note + note_counter)->note_strength = (double)0.0;
    }

  for (bucket_counter = 0; bucket_counter < TOTAL_BUCKETS; bucket_counter += 1)
    {
    current_bucket = my_bucket_data + bucket_counter;
    current_bucket->bucket_strength = goertzel(current_bucket->buffer_size,
                             current_bucket->bucket_frequency,
                             SAMPLE_RATE,
                             full_size_buffer);
    current_bucket->this_piano_note->note_strength += current_bucket->bucket_strength;

    //Test print
    fprintf(result_plot_file,
       "%d   %f\n",
       bucket_counter, current_bucket->bucket_strength);
    }

  working_note = my_piano_notes;
  for (note_counter = 0; note_counter < TOTAL_NOTES; note_counter += 1)
    {
    fprintf(note_result_plot_file, "%d   %f\n", note_counter, (working_note + note_counter)->note_strength);
    }

  fclose (note_result_plot_file);

  //now find the strongest note
  strongest_note = my_piano_notes;
  working_note = my_piano_notes;

  for (note_counter = 0; note_counter < TOTAL_NOTES; note_counter += 1)
    {
    if (working_note->note_strength > strongest_note->note_strength)
      {
      strongest_note = working_note;
      }
    working_note += 1;
    }
  my_piano_data_struct.keyboard_key = strongest_note->keyboard_key;
  my_piano_data_struct.octive = strongest_note->octive;
  my_piano_data_struct.done = 1;

    
  fclose (result_plot_file);
  
  // printf ("done process \n");
  return &my_piano_data_struct;
  }

int piano_process_class::initialize_piano_data()
  {
  double current_bucket_frequency;
  double previous_bucket_frequency;
  int current_octive;
  int current_bucket;
  int current_note;
  int bucket_in_note;
  int note_in_octive;
  double semi_interval;
  struct bucket_data_struct *current_bucket_pointer;
  struct piano_note_struct *current_note_pointer;
  FILE *print_records;
  int array_counter;
  
  note_in_octive = 0; 
  current_bucket_frequency = (double)32.0;
  previous_bucket_frequency = (double)32.0;
  current_note = 0;
  current_octive = 0;
  bucket_in_note = 0;

  semi_interval = (double)(pow(2.0, 1.0/(double(BUCKETS_PER_OCTIVE))));

  // printf("semi is %f\n", semi_interval);

  print_records = fopen("my_bucket_record","w");
  if (print_records == NULL)
    {
    printf ("cannot open records file \n");
    }

  // initialize full_size_buffer
  for (array_counter = 0; array_counter < PIANO_FULL_ARRAY_SIZE; array_counter += 1)
    {
    full_size_buffer[array_counter] = (Sint16)0;
    }

  main_buffer_read_count = 0;

  for (current_bucket = 0; current_bucket < TOTAL_BUCKETS; current_bucket += 1)
    {
    // Set pointers to bucket structure
    current_bucket_pointer = my_bucket_data + current_bucket;
    // Set pointer to note
    current_note_pointer = my_piano_notes + (int)(current_bucket / BUCKETS_PER_NOTE) + bucket_in_note;
    // Fill out both bucket and note structures
    current_bucket_pointer->this_piano_note = current_note_pointer;
    current_bucket_pointer->bucket_strength = 0.0;
    current_note_pointer->note_strength = 0.0;
    current_note_pointer->keyboard_key = note_in_octive;
    current_note_pointer->octive = current_octive;

    //Set current bucket frequency
    //First is handled differently
    if (current_bucket == 0)
      {
      current_bucket_pointer->bucket_frequency = current_bucket_frequency;
      previous_bucket_frequency = current_bucket_frequency;
      }

    else
      {
      current_bucket_frequency = previous_bucket_frequency * semi_interval;
      previous_bucket_frequency = current_bucket_frequency;
      }
    
    current_bucket_pointer->bucket_frequency = current_bucket_frequency;

    // Radians on cycle per sample 
    current_bucket_pointer->radians_per_sample = (double)RADIANS_PER_SAMPLE_AT_1_HZ *
      (double)current_bucket_pointer->bucket_frequency;

    // Buffer size for this bucket's frequency
    current_bucket_pointer->buffer_size = (int)((double)(SAMPLE_RATE)/
      current_bucket_pointer->bucket_frequency) * Q_QUALITY;

     //Verification prints
    fprintf (print_records, "current bucket - %d; bucket in note - %d; note in octive - %d; current note %d; current octive - %d ; bucket_frequency - %f ; degree per sample - %f; buffer size - %d; which note - %d\n",
     current_bucket, bucket_in_note, note_in_octive, current_note, current_octive,
     current_bucket_pointer->bucket_frequency, current_bucket_pointer->radians_per_sample,
     current_bucket_pointer->buffer_size,
     (int)(current_bucket / BUCKETS_PER_NOTE));

    // Bump up for next; reset if necessary
    // Note frequency 

    // Update bucket in note and note in octive
    bucket_in_note += 1;
    if (bucket_in_note == BUCKETS_PER_NOTE)
      {
      bucket_in_note = 0;
      current_note += 1;
      note_in_octive += 1;
      if (note_in_octive == NOTES_PER_OCTIVE)
        {
        note_in_octive = 0;
        current_octive += 1;
        }
      }
    }

  fclose(print_records);
  printf ("done initialize \n");
  return 0;
  }