clear;

%%%%%%%%
% "constants" %
%%%%%%%%

% how many updates per second, determines the length of audio snippets
const_frames_per_second = 100;

const_Fs = 44100;  % sampling rate in Hz
const_te = 1/const_frames_per_second; % signal duration in seconds
const_samples_per_frame = ceil(const_Fs * const_te);
const_sample_range = 0:const_samples_per_frame-1;

% factor the last read frame is multiplied by
const_fade_speed = 0.975;

% signal "generator"
signal = @(freq) (freq ./ const_Fs .* 2 .* pi .* const_sample_range);

% time given for calculation
const_calc_offset = 0;

% dimensions to use for detecting number of hands in frame
zero_hands = size(NaN(0,0));
one_hand = size(NaN(1,1));
two_hands = size(NaN(1,2));

deviceWriter = audioDeviceWriter('SampleRate', const_Fs, 'SupportVariableSizeInput', true, 'BufferSize', 3 * const_samples_per_frame);

%%%%%%%%%%%%%%%%%%%
% init matleap by calling for first frame %
%%%%%%%%%%%%%%%%%%%
matleap_frame

% runtime variables
P = NaN(1000000,3);
count = 1;
done = false;

x_one = 0;
x_two = 0;

y_one = 0;
y_two = 0;

offset_one = 0;
offset_two = 0;

signal_one = sin(signal(0));
signal_two = sin(signal(0));

full_signal = signal_one + signal_two;
complete_signal = full_signal;

% time gestures, used for program termination
gesture_count = 0;

%while count < 100000 && gesture_count < 3
while gesture_count < (1 * (const_frames_per_second - const_calc_offset))
    frame = matleap_frame;
    handCount = size(frame.hands);
    
    if isequal(zero_hands, handCount)
        gesture_count = 0;
        
        % slowly lower volume if no hand is there
        [signal_one, offset_one] = get_theremin_sound_bit(x_one, y_one * const_fade_speed, offset_one, signal);
        [signal_two, offset_two] = get_theremin_sound_bit(x_two, y_two * const_fade_speed, offset_two, signal);
        
        % TODO
    elseif isequal(one_hand, handCount)
        % fade hand two out
        [signal_two, offset_two] = get_theremin_sound_bit(x_two, y_two * const_fade_speed, offset_two, signal);
        
        pos = frame.hands(1).palm.position;
        %pos = frame.hands(1).palm.stabilized_position;
        
        x_one = pos(1);
        y_one = pos(2);
        
        [signal_one, offset_one] = get_theremin_sound_bit(x_one, y_one, offset_one, signal);
        P(count, 1:3) = pos;
        count = count + 1;
    
        if frame.gesture > 0
            gesture_count = gesture_count + 1;
        else
            gesture_count = 0;
        end
    elseif isequal(two_hands, handCount)
        gesture_count = 0;
        
        pos = frame.hands(1).palm.position;
        %pos = frame.hands(1).palm.stabilized_position;
        
        x_one = pos(1);
        y_one = pos(2);
        
        [signal_two, offset_two] = get_theremin_sound_bit(x_two, y_two * const_fade_speed, offset_two, signal);
        P(count, 1:3) = pos;
        count = count + 1;
        
        pos = frame.hands(2).palm.position;
        %pos = frame.hands(2).palm.stabilized_position;

        x_two = pos(1);
        y_two = pos(2);
        
        [signal_two, offset_two] = get_theremin_sound_bit(x_two, y_two, offset_two, signal);
    else
        disp("That should not happen:");
        disp(handCount);
        return;
    end
    
    % play current sound
    full_signal = signal_one + signal_two;
    if deviceWriter(full_signal(:)) ~= 0
        disp("Buffer ran empty!");
    end
    complete_signal = [complete_signal full_signal];
    
    % enforce frame rate, leaving a bit of room for calculation
    %java.lang.Thread.sleep((1/(const_frames_per_second - const_calc_offset)) * 1000);
end

release(deviceWriter)

%theremin_player = audioplayer(complete_signal, const_Fs);
%play(theremin_player);

% extract values
x = P(:,1); % links (-) rechconst_Ts (+) (LED zu uns)
y = P(:,2); % oben unten
z = P(:,3); % vorne (+) hinten (-) (LED zu uns)

% plot
figure("Position",[0,0, 1200, 2400]);
t = tiledlayout(4,1);

nexttile;
plot(x);
ylabel('left right');

nexttile;
plot(y);
ylabel('height');

nexttile;
plot(z);
ylabel('depth');

nexttile;
plot3(z,x,y);
xlabel('depth');
ylabel('left right');
zlabel('height');

function [sound,offset] = get_theremin_sound_bit(x, y, offset, generator)
    % finding ranges for x and y:
    %'x'
    %min(x)
    %max(x)
    %'y'
    %min(y)
    %max(y)
    % --> -300 < x < 300
    % --> 0 < y < 600
    
    % therefore
    volume = y / 2000;
    frequency = max(0.0001, (x + 400) / 1200) * 3000; % https://web.physics.ucsb.edu/~lecturedemonstrations/Composer/Pages/60.17.html # How it works

    base = generator(frequency) + offset;
    
    offset = base(end);
    
    sound = sin(base) .* volume;
end