%这里取D的单位为1e-12，t 1e-6，r 1e-9
%D=2e-15
%1表示 DONOR_PHOTON 2表示ACCEPTOR_PHOTON 0表示没有信号
tic
diffusion_coefficient=2e-3;
delta_t=10;
foerster_radius=6.5;
min_dis=0;
max_dis=2*foerster_radius;

obtain_prob=0.1;%观测到数据的概率

%仿真
t_mem=[0:delta_t:1e4]';
r_mem=zeros(size(t_mem));
y_mem=zeros(size(t_mem));
r_mem(1)=foerster_radius+randn(1,1);
while(abs(r_mem(1)/foerster_radius-1)>0.5)
	r_mem(1)=foerster_radius+randn(1,1);
	E=1/(1+(r_mem(1)/foerster_radius)^6);
	if(rand<E)
		y_mem(1)=2;
	else
		y_mem(1)=1;
	end
end
for i=2:length(t_mem)
	r_mem(i)=r_mem(i-1)+randn(1,1)*sqrt(2*diffusion_coefficient*delta_t);
	while(r_mem(i)<0.1*foerster_radius)
		r_mem(i)=r_mem(i-1)+randn(1,1)*sqrt(2*diffusion_coefficient*delta_t);
	end
	E=1/(1+(r_mem(i)/foerster_radius)^6);
	if(rand<E)
		y_mem(i)=2;
	else
		y_mem(i)=1;
	end
	if(rand>obtain_prob)
		y_mem(i)=0;
	end
end
I=find(y_mem);
t_nz_mem=t_mem(I);
y_nz_mem=y_mem(I);

%基于强度的平滑算法
idsmoothing_mem=zeros(size(t_mem));
time_window=1e3;
grid_half_window=round(time_window/delta_t/2);
grid_num=101;
r_grid=([1:grid_num]')/grid_num*(max_dis-min_dis)+min_dis;
w_grid=zeros(grid_num,1);
for i=1:length(t_mem)
	l=max(1,i-grid_half_window);
	r=min(length(t_mem),i+grid_half_window);
	if length(find(y_mem(l:r)))>0
		E=sum(y_mem(l:r))/length(find(y_mem(l:r)));
	else
		E=0;
	end
	if E<0.1
		idsmoothing_mem(i)=foerster_radius;
	elseif E-1<1/(1+(max_dis/foerster_radius)^6)
		idsmoothing_mem(i)=max_dis;
	else
		E=E-1;
		idsmoothing_mem(i)=(1/E-1)^(1/6)*foerster_radius;
	end
end

%基于强度的贝叶斯平滑算法
ibsmoothing_mem=zeros(size(t_mem));
time_window=1e3;
grid_half_window=round(time_window/delta_t/2);
grid_num=200;
r_grid=([1:grid_num]')/grid_num*(max_dis-min_dis)+min_dis;
w_grid=zeros(grid_num,1);
for i=1:length(t_mem)
	l=max(1,i-grid_half_window);
	r=min(length(t_mem),i+grid_half_window);
	w_grid=(r_grid/foerster_radius).^5./(1+(r_grid/foerster_radius).^6).^2;
	w_grid=log(w_grid)+sum(y_mem(l:r)==1)*log((r_grid/foerster_radius).^6./(1+(r_grid/foerster_radius).^6))+sum(y_mem(l:r)==2)*log(1./(1+(r_grid/foerster_radius).^6));
	w_grid=w_grid-max(w_grid);
	w_grid=exp(w_grid);
	ibsmoothing_mem(i)=w_grid'*r_grid/sum(w_grid);
end

%新粒子平滑算法
newmax_dis=100;%1.9*foerster_radius;
newmin_dis=0.1*foerster_radius;
particle_num=200;
pssmoothing_mem=zeros(size(t_mem));
forward_mem=zeros(particle_num,length(t_mem));
forward_w_mem=zeros(particle_num,length(t_mem));
backward_mem=zeros(particle_num,length(t_mem));
backward_w_mem=zeros(particle_num,length(t_mem));
tmp_mem=zeros(particle_num,1);
tmp_w_mem=zeros(particle_num,1);
%正向滤波
for i=1:length(t_mem)
	if(i==1)
		forward_mem(:,i)=rand(particle_num,1)*foerster_radius+0.5*foerster_radius;
		tmp_mem=forward_mem(:,i);
		tmp_t=i;
		tmp_w_memory=1/particle_num*ones(particle_num,1);
	else
		forward_mem(:,i)=tmp_mem+randn(particle_num,1)*sqrt(2*diffusion_coefficient*delta_t*(i-tmp_t));
		II=find(forward_mem(:,i)<newmin_dis|forward_mem(:,i)>newmax_dis);
		while(length(II)>0)
			forward_mem(II,i)=tmp_mem(II)+randn(length(II),1)*sqrt(2*diffusion_coefficient*delta_t*(i-tmp_t));
			II=find(forward_mem(:,i)<newmin_dis|forward_mem(:,i)>newmax_dis);
		end
	end
	forward_w_mem(:,i)=tmp_w_memory;
	if(y_mem(i)~=0)
		tmp_mem=forward_mem(:,i);
		tmp_t=i;
		if(y_mem(i)==1)
			forward_w_mem(:,i)=forward_w_mem(:,i).*(1-1./(1+(forward_mem(:,i)/foerster_radius).^6));
		else
			forward_w_mem(:,i)=forward_w_mem(:,i)./(1+(forward_mem(:,i)/foerster_radius).^6);
		end
		%归一化
		forward_w_mem(:,i)=forward_w_mem(:,i)/sum(forward_w_mem(:,i));
		tmp_w_memory=forward_w_mem(:,i);
		if(1/sum(forward_w_mem(:,i).^2)<particle_num)
			%重采样
			Q=zeros(1,particle_num+1);
			for k=2:particle_num+1
				Q(k)=Q(k-1)+forward_w_mem(k-1,i);
			end
			Q(particle_num+1)=1;
			t=-log(rand(1,particle_num+1));
			T=t;
			for k=2:particle_num+1
				T(k)=T(k-1)+t(k);
			end
			l=0;
			j=0;
			k=1;
			while j<=particle_num&&k<=particle_num
				if(Q(j+1)*T(particle_num+1)>T(l+1))
					l=l+1;
					tmp_mem(k)=forward_mem(j,i);
					k=k+1;
				else
					j=j+1;
				end
			end
			tmp_w_memory=1/particle_num*ones(particle_num,1);
		end
	end
	pssmoothing_mem(i)=forward_w_mem(:,i)'*forward_mem(:,i);
end
%反向滤波
gamma=foerster_radius;%人工分布参数
for i=length(t_mem):-1:1
	%采样
	if(i==length(t_mem))
		backward_mem(:,i)=forward_mem(:,i);
		tmp_mem=backward_mem(:,i);
		tmp_t=i;
		tmp_w_memory=1/particle_num*ones(particle_num,1);
	else
		backward_mem(:,i)=tmp_mem+randn(particle_num,1)*sqrt(2*diffusion_coefficient*delta_t*abs(tmp_t-i));
		II=find(backward_mem(:,i)<newmin_dis|backward_mem(:,i)>newmax_dis);
		while(length(II)>0)
			backward_mem(II,i)=tmp_mem(II)+randn(length(II),1)*sqrt(2*diffusion_coefficient*delta_t*abs(tmp_t-i));
			II=find(backward_mem(:,i)<newmin_dis|backward_mem(:,i)>newmax_dis);
		end
	end
	%计算权值
	tmp_w_mem=tmp_w_memory.*(backward_mem(:,i)/gamma).^5./(1+(backward_mem(:,i)/gamma).^6).^2;%gamma./(backward_mem(:,i).^2+gamma^2);
	if(y_mem(i)==1)
		tmp_w_mem=tmp_w_mem.*(1-1./(1+(backward_mem(:,i)/foerster_radius).^6));
	elseif(y_mem(i)==2)
		tmp_w_mem=tmp_w_mem./(1+(backward_mem(:,i)/foerster_radius).^6);
	end
	if(i<length(t_mem))
		tmp_w_mem=tmp_w_mem./((tmp_mem/gamma).^5./(1+(tmp_mem/gamma).^6).^2);%tmp_w_mem./(gamma./(tmp_mem.^2+gamma^2));
	end
	%归一化
	tmp_w_mem=tmp_w_mem/sum(tmp_w_mem);
	backward_w_mem(:,i)=tmp_w_mem;
	if(y_mem(i)~=0|i==length(t_mem))
		tmp_mem=backward_mem(:,i);
		tmp_t=i;
		tmp_w_memory=backward_w_mem(:,i);
		if(1/sum(backward_w_mem(:,i).^2)<particle_num)
			%重采样
			Q=zeros(1,particle_num+1);
			for k=2:particle_num+1
				Q(k)=Q(k-1)+backward_w_mem(k-1,i);
			end
			Q(particle_num+1)=1;
			t=-log(rand(1,particle_num+1));
			T=t;
			for k=2:particle_num+1
				T(k)=T(k-1)+t(k);
			end
			l=0;
			j=0;
			k=1;
			while j<=particle_num&&k<=particle_num
				if(Q(j+1)*T(particle_num+1)>T(l+1))
					l=l+1;
					tmp_mem(k)=backward_mem(j,i);
					k=k+1;
				else
					j=j+1;
				end
			end
			tmp_w_memory=1/particle_num*ones(particle_num,1);
		end
	end
	pssmoothing_mem(i)=backward_w_mem(:,i)'*backward_mem(:,i);
end
%综合
for i=1:length(t_mem)
	if(mod(i,100)==1)
		i
	end
	tmp_w_mem=backward_w_mem(:,i)./((backward_mem(:,i)/gamma).^5./(1+(backward_mem(:,i)/gamma).^6).^2);%(gamma./(backward_mem(:,i).^2+gamma^2));
	if(i==1)
		tmp_w_mem=tmp_w_mem.*(backward_mem(:,i)>newmin_dis&backward_mem(:,i)<newmax_dis);
	else
		tmp_w_matrix=-(ones(particle_num,1)*forward_mem(:,i-1)'-backward_mem(:,i)*ones(1,particle_num)).^2/(4*diffusion_coefficient*delta_t);
		tmp_w_matrix=tmp_w_matrix-max(max(tmp_w_matrix));
		tmp_w_matrix=exp(tmp_w_matrix);
		tmp_w_mem=tmp_w_mem.*(tmp_w_matrix*forward_w_mem(:,i-1));
	end
	tmp_w_mem=tmp_w_mem/sum(tmp_w_mem);
	pssmoothing_mem(i)=backward_mem(:,i)'*tmp_w_mem;
end

%格点平滑算法
grid_num=200;
gssmoothing_mem=zeros(size(t_mem));
grid_mem=[1:grid_num]'/grid_num*(max_dis-min_dis)+min_dis;
forward_mem=zeros(grid_num,length(t_mem));
forward_w_mem=zeros(grid_num,length(t_mem));
backward_mem=zeros(grid_num,length(t_mem));
backward_w_mem=zeros(grid_num,length(t_mem));
tmp_mem=zeros(grid_num,1);
tmp_w_mem=zeros(grid_num,1);
tmp_w_matrix=-(ones(grid_num,1)*grid_mem'-grid_mem*ones(1,grid_num)).^2/(4*diffusion_coefficient*delta_t);
tmp_w_matrix=tmp_w_matrix-max(max(tmp_w_matrix));
tmp_w_matrix=exp(tmp_w_matrix);
%正向滤波
for i=1:length(t_mem)
	if(y_mem(i)==0)
		forward_w_mem(:,i)=1;
	elseif(y_mem(i)==1)
		forward_w_mem(:,i)=1-1./(1+(grid_mem/foerster_radius).^6);
	else
		forward_w_mem(:,i)=1./(1+(grid_mem/foerster_radius).^6);
	end
	if(i>1)
		forward_w_mem(:,i)=forward_w_mem(:,i).*(tmp_w_matrix*forward_w_mem(:,i-1));
	end
	forward_w_mem(:,i)=forward_w_mem(:,i)/sum(forward_w_mem(:,i));
end
%反向滤波
for i=length(t_mem):-1:1
	if(y_mem(i)==0)
		backward_w_mem(:,i)=1;
	elseif(y_mem(i)==1)
		backward_w_mem(:,i)=1-1./(1+(grid_mem/foerster_radius).^6);
	else
		backward_w_mem(:,i)=1./(1+(grid_mem/foerster_radius).^6);
	end
	if(i<length(t_mem))
		backward_w_mem(:,i)=backward_w_mem(:,i).*(tmp_w_matrix*backward_w_mem(:,i+1));
	end
	backward_w_mem(:,i)=backward_w_mem(:,i)/sum(backward_w_mem(:,i));
end
%综合
for i=1:length(t_mem)
	tmp_w_mem=backward_w_mem(:,i);
	if(i>1)
		tmp_w_mem=tmp_w_mem.*(tmp_w_matrix*forward_w_mem(:,i-1));
	end
	tmp_w_mem=tmp_w_mem/sum(tmp_w_mem);
	gssmoothing_mem(i)=grid_mem'*tmp_w_mem;
end

figure;
plot(t_mem,r_mem,'--');
hold on;
plot(t_mem,idsmoothing_mem);
hold off;

figure;
plot(t_mem,r_mem,'--');
hold on;
plot(t_mem,ibsmoothing_mem);
hold off;

figure;
plot(t_mem,r_mem,'--');
hold on;
plot(t_mem,pssmoothing_mem);
hold off;

figure;
plot(t_mem,r_mem,'--');
hold on;
plot(t_mem,gssmoothing_mem);
hold off;

sum((idsmoothing_mem-r_mem).^2)/1001
sum((ibsmoothing_mem-r_mem).^2)/1001
sum((pssmoothing_mem-r_mem).^2)/1001
sum((gssmoothing_mem-r_mem).^2)/1001
toc