%% just a script for messing around
%% with trying to clean up lab data
%% Only for testing our methods
clear all
close all


time1=linspace(0,2*pi,100);
time2=linspace(2*pi,4*pi,100);
time2=time2(2:end);
time3=linspace(4*pi,6*pi+pi/2,100);
time3=time3(2:end);
time4=linspace(6*pi+pi/2,9*pi,100);
time4=time4(2:end);

angle1=pi*cos(time1*2)+0.3;
angle2=-pi*cos(time2*2)+2*pi+0.3;
angle3=pi*cos(time3*2)+0.3;
angle4=-pi*cos(time4*2)-2*pi+0.3;

time=[time1,time2,time3,time4]';
theta=[angle1,angle2,angle3,angle4]';

pi_t=pi+pi/4;

%%find spikes in data and add appropriate multiples of 2*pi
deriv2=[0;theta(1:end-2)-2*theta(2:end-1)+theta(3:end);0];
%spike_spots=[0;(abs(deriv2)>1)];
spike_spots=[0;(abs(diff(theta))>pi_t)];
spike_index=[find(spike_spots);length(spike_spots)];
spike=-round([0;(abs(diff(theta))>pi_t).*diff(theta)]/2/pi);

test=[0;(abs(diff(theta))>pi_t).*diff(theta)];

num_spikes=sum(abs(spike_spots));
spike_free_theta=theta;

n=0;

for ii=1:num_spikes
    chunk=[spike_index(ii):spike_index(ii+1)];
    n=n+spike(spike_index(ii));
    spike_free_theta(chunk)=theta(chunk)+2*pi*n*abs(spike(spike_index(ii)));
end

% %repeat a similar process
% spike_spots2=[(abs((spike_free_theta))>4.5).*sign(spike_free_theta)];
% spike_index2=[find(spike_spots2);length(spike_spots2)];
% spike2=round([(abs(spike_free_theta)>4.5).*spike_free_theta]/2/pi);
% 
% 
% 
% diffspike2=[0;diff(spike2)];
% diffspike2(find(diffspike2==0))=1;
% 
% %spike_free_theta2=spike_free_theta-spike_spots2*2*pi;
% spike_free_theta2=spike_free_theta-spike2*2*pi;


figure(1)
plot(time,theta,'-o',time,spike_spots,'o',time,test,'+',[time(1),time(end)],[pi,pi],[time(1),time(end)],[-pi,-pi])

figure(2)
plot(time, spike_free_theta, time,theta)

figure(3)
plot(time,theta,time,spike_free_theta,time, deriv2)

% figure(4)
% plot(time, spike_free_theta,'-o',time, spike2,'+',time, spike_free_theta2,'-o',time,diffspike2,'-o')

return

theta_orig=[linspace(0,4*pi,100),linspace(4*pi,0,100)]';
time=[1:length(theta_orig)];

r=5;

x=r*cos(theta_orig);
y=r*sin(theta_orig);

theta=atan2(y,x);

%spike_index_plus=find(diff(theta)>pi+0.1);
%spike_index_minus=find(diff(theta)<-pi-0.1);
spike_spots=[0;(abs(diff(theta))>pi+0.1)];
spike_index=[find(spike_spots);length(spike_spots)];
spike=-round([0;(abs(diff(theta))>pi+0.1).*diff(theta)]/2/pi);

%num_spikes=min([length(spike_index_plus),length(spike_index_minus)])-1;
num_spikes=sum(abs(spike_spots));
spike_free_theta=theta;

n=0;

for ii=1:num_spikes
    chunk=[spike_index(ii):spike_index(ii+1)];
    spike(spike_index(ii))
    n=n+spike(spike_index(ii));
    spike_free_theta(chunk)=theta(chunk)+2*pi*n*abs(spike(spike_index(ii)));
end

figure(1)
plot(time,theta_orig,time,theta)

figure(2)
plot(time,theta,'-o',time,spike_spots,'o',time,spike,'+',[time(1),time(end)],[pi,pi],[time(1),time(end)],[-pi,-pi])

figure(3)
plot(time,theta,'-o',time(1:length(spike_free_theta)),spike_free_theta,'-+')

