306 lines
9.1 KiB
C++
306 lines
9.1 KiB
C++
#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;
|
|
}
|
|
|