%初始化三角模式中的三个点
rng(0);

% delta = 5;
delta = 2.5;
%随机选择一个方向探索
% phi_0 = (rand()*2-1)*pi;
phi_0 = 0;
delta_phi_0 = [2*pi/3 -2*pi/3];
n = 300;
x = zeros(n,2);
xinit = [0 0];  
x(1,:) = delta*[cos(phi_0) sin(phi_0)]+xinit;
% x(1,:) = delta*[0 sin(phi_0)]+xinit;
a = phi_0 + delta_phi_0;
x(2:3,:) = delta*[cos(a)' sin(a)']+xinit;
rpos = [-0.5,0.5];
% rpos = [0.5,0.5];
waveenv = WaveEnv();
yfun = @(angles,k)waveenv.getIntensityByYP(rpos(1),rpos(2),angles(1),angles(2),0/100);
% yfun([0,0],0)
static_intensity = zeros(n,1);
y = zeros(n,1);
static_intensity(1) = yfun(xinit,1);
static_intensity(1)
%%
static_intensity(2) = yfun(xinit,2);
y(1) = yfun(x(1,:),1);
y(2) = yfun(x(2,:),2);
%   midpoint -0.2362    0.1732    1.1345
%    -0.1732    0.2992    1.0582
for k = 3:n
    y(k) = yfun(x(k,:),k);
    static_intensity(k) = yfun(xinit,k);
    delta_y = (y(k)+y(k-1))/2 - y(k-2);
    if delta_y >= 0
        uk = x(k-1,:) - x(k-2,:);
    else
        uk = x(k-2,:) - x(k,:);
    end
%     uk = uk / norm(uk) * step_length;
    x(k+1,:) = x(k,:) + uk;
end
figure
subplot(3,1,1)
plot(y)
hold on   
plot(static_intensity)
hold off
legend('modified','static')
subplot(3,1,2)
% for i = 1:size(x,1) 
%     if x(i,2) > 90
%         x(i,2) = -90 + x(i,2);
%     elseif x(i,2)<-90
%         x(i,2) = 90 -x(i,2);
%     end
%     if x(i,1) < 0
%         x(i,1) = 360+x(i,1);
%     elseif x(i,1)>360
%         x(i,1) = x(i,1) - 360;
%     end
% end
figure()
plot(x(:,1))
hold on
plot(x(:,2))
xlim([0,n])
legend('Yaw Angle of TE','Pitch Angle of TE')
xlim([0,n])
% tl = 1:1:n;
% ints = zeros(n,1);
% for t = tl
%     ints(t) = intensityObjFun(x(end,1),x(end,2),t);
% end
% figure
% semilogy(ints)
mean(x(200:end,:))
% divergence value 
%  223.3858   20.7546 1.625e-7
% save te x y;