clear; clc
warning('off');

par = {'ham37_LeftDG_004';
       'ham37_RightCA1_005'};
ip = 1; icsv = 1; sr = 1250;
    
intandata = dir([par{ip,1},'_*']);
info = read_Intan_RHD2000_file([intandata.folder,'\',intandata.name]);
% digitalin
digital = read_Intan_digitalin_file([intandata.folder,'\',intandata.name,'\digitalin.dat'],info.num_board_dig_in_channels);
timss(icsv,3) = size(digital(:,1),1)/info.sample_rate;
timss(icsv,1) = find(digital(:,1),1,'first')/info.sample_rate;
timss(icsv,2) = find(digital(:,1),1,'last')/info.sample_rate;
% time
prefileinfo = dir([intandata.folder,'\',intandata.name,'\time.dat']);
num_samples = prefileinfo.bytes/4; % int32 = 4 bytes
fid = fopen([intandata.folder,'\',intandata.name,'\time.dat'], 'r');
tim = fread(fid, num_samples, 'int32'); fclose(fid); tim = tim / info.sample_rate; 
% analogin
v = read_Intan_analogin_file([intandata.folder,'\',intandata.name,'\analogin.dat'],info.num_board_adc_channels);
% cs
csv{icsv} = [par{ip,1},'.csv']; mvt = 1;
optitrack.efps = read_csv_Export_FPS(csv{icsv});
optitrack.pos = csvread(csv{icsv},7,1);
optitrack.pos(:,1) = optitrack.pos(:,1) + timss(icsv,1);
idx = find(optitrack.pos(:,2)==0);
optitrack.pos(idx,2:8) = nan(size(idx,1),7);
optitrack.pos = fillmissing(optitrack.pos,'linear');
t = optitrack.pos(:,1);
x = optitrack.pos(:,6);
y = optitrack.pos(:,8);
z = optitrack.pos(:,7);
vel = [0;0;sqrt(sum(diff([x,y],2).^2,2))*optitrack.efps];
vel = smoothdata(vel,'movmean',optitrack.efps*mvt);

v = downsample(v',info.sample_rate/sr);
tim = downsample(tim,info.sample_rate/sr);
sigfiltered = bz_Filter([tim v(:,2)],'passband',[0,12],'filter','butter');
% signal = detrend(sigfiltered(:,2));
p = polyfit(tim, sigfiltered(:,2), 1);
signal = (sigfiltered(:,2) - polyval(p, tim))./polyval(p, tim);

%% plot
fig = figure();
subplot(2,1,1)
plot(t + timss(icsv,1),vel,'r'); xlim([timss(icsv,1),max(tim)]); ylim([0,0.4])
box off; axis on; title("Speed");
subplot(2,1,2)
plot(tim,signal,'b'); xlim([timss(icsv,1),max(tim)]); ylim([-0.1,0.1]);
box off; axis on; title("Signal (polynomial detrended)");
savefig(fig,[par{ip,1},'.fig'],'compact');

tt = suptitle(par{ip,1}); set(tt,'Interpreter','none');

