%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Hypnodyne ZMax connection test
%
%   Grabs raw data in real time for whatever application
%   Updated June 5, 2018 
%
%   hypnodynecorp.com
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function HDConnect()

% connect to HDServer

t = tcpip('localhost', 8000); %use the HDServer internet address and port
set(t, 'InputBufferSize', 5000); 
fopen(t); 
fprintf(t, 'HELLO'); %server starts sending us data when greeted
pause(0.3);

buf_size = 3 * 256; %3 seconds at 256 frames per second (matlab plotting is SLOW)

% initialize buffers

buf_eeg1 = zeros(buf_size, 1);
buf_eeg2 = zeros(buf_size, 1); 
buf_dx = zeros(buf_size, 1);
buf_dy = zeros(buf_size, 1);
buf_dz = zeros(buf_size, 1);

% initialize plot

close all; 
clc;
hf = figure;

%section one, EEG
 
subplot(2,1,1);
hEEG1 = line(1:buf_size,buf_eeg1,'color', [0 0.5 1]);
hold on;
hEEG2 = line(1:buf_size,buf_eeg2,'color', [0.2 0.7 0.8]);
ylim([-400 400]); 
title('EEG');

%section two, accelerometer

subplot(2,1,2);
hdx = line(1:buf_size,buf_dx,'color', [0 0.5 0]);
hold on;
hdy = line(1:buf_size,buf_dy,'color', [0 0.5 0.2]);
hdz = line(1:buf_size,buf_dz,'color', [0.2 0.5 0]);
ylim([-2 2]);
title('Accelerometer');

set(gcf,'GraphicsSmoothing','off'); %try to speed it up but it's hopeless ;)
set(gcf,'currentchar',' ');
k = 0;

while ((ishandle(hf) && get(gcf,'currentchar')==' '))  
    buf = fscanf(t);
    if (startsWith(buf,'DEBUG')) %ignore debugging messages from server
    else
        if (startsWith(buf,'D')) %only process data packets
            p = strsplit(buf,'.');
            if (size(p,2)==2)
                buf = p{2};
                packet_type=getbyteat(buf,0);
                if ((packet_type >= 1) && (packet_type <= 11)) %packet type within correct range
                    if (size(buf,2) == 121) %properly formatted data line
                        
                        % EEG channels
                        
                        eegr = getwordat(buf, 1);
                        eegl = getwordat(buf, 3);
                        
                        % Accelerometer channels
                        
                        dx = getwordat(buf, 5);
                        dy = getwordat(buf, 7);
                        dz = getwordat(buf, 9); 
                        
                        % PPG channels (not plotted to keep display fast)
                        
                        oxy_ir_ac = getwordat(buf, 27); 
                        oxy_r_ac = getwordat(buf, 25); %requires external nasal sensor
                        oxy_dark_ac = getwordat(buf, 34); %requires external nasal sensor
                        oxy_ir_dc =  getwordat(buf, 17); %requires external nasal sensor
                        oxy_r_dc =  getwordat(buf, 15); %requires external nasal sensor
                        oxy_dark_dc = getwordat(buf, 32); %requires external nasal sensor
                        
                        % other channels (not plotted to keep display fast)
                        
                        bodytemp = getwordat(buf, 36);
                        nasal_l = getwordat(buf, 11); %requires external nasal sensor
                        nasal_r = getwordat(buf, 13); %requires external nasal sensor
                        light = getwordat(buf, 21);
                        bat = getwordat(buf, 23);
                        noise = getwordat(buf, 19); 
                        
                        % fill our buffer with data (wrap when full)
                        k = k + 1;
                        if (k > buf_size) 
                            k = 1;
                        end
                        buf_eeg1(k) = ScaleEEG(eegr); %gets uV value
                        buf_eeg2(k) = ScaleEEG(eegl); 
                        buf_dx(k) = ScaleAccel(dx); %gets 'g' value
                        buf_dy(k) = ScaleAccel(dy);  
                        buf_dz(k) = ScaleAccel(dz); 
                        
                        if (mod(k, 30) == 0) %update display every 30 packets
                            set(hEEG1, 'YData', buf_eeg1);
                            set(hEEG2, 'YData', buf_eeg2);  
                            set(hdx, 'YData', buf_dx); 
                            set(hdy, 'YData', buf_dy); 
                            set(hdz, 'YData', buf_dz); 
                            drawnow; 
                        end
                    end
                end
            end
        end
    end 
end 
fclose(t); 
delete(t); 
clear t;
close all;
end

function [d] = ScaleEEG(e) %word value to uV 
    uvRange = 3952;
    d = e - 32768;
    d = d * uvRange;
    d = d / 65536;
end
function [d] = ScaleAccel(dx) %word value to 'g'
    d = dx * 4 / 4096 - 2;
end
function [v] = BatteryVoltage(vbat) %word value to Volts
    v = vbat/1024*6.60;  
end
function [t] = BodyTemp(bodytemp) %word value to degrees C
    v = bodytemp / 1024 * 3.3;
    t = 15 + ((v - 1.0446) / 0.0565537333333333); 
end
function [w] = getwordat(buf, i)
    w = getbyteat(buf,i) * 256 + getbyteat(buf,i+1);
end
function [s] = getbyteat(buf, i)  
    s = buf(i*3+1:i*3+1+1);
    s = hex2dec(s);
end