load('wavedisagent-best-best.mat','agent');
Ts = 0.1;
Tf = 20;
rpos = [-1,1];
env = WaveEnvDiscreteAction(rpos,true);
simOptions = rlSimulationOptions('MaxSteps',ceil(Tf/Ts));
experience = sim(env,agent,simOptions);


%初始化三角模式中的三个点
% close all;
% rng(0);
% delta = 5;
% delta = 2.5; 默认值
% delta = 5;

delta = 6;
% delta = 8;
%随机选择一个方向探索
% phi_0 = (rand()*2-1)*pi;
phi_0 = 0;
phi_0 = -30;

% phi_0 = -130;
R = [ cosd(phi_0) -sind(phi_0);
        sind(phi_0) cosd(phi_0)];
% phi_0 = pi/3;
% delta_phi_0 = [2*pi/3 -2*pi/3];
n = 200;
% n = 1000;
x = zeros(n,2);
xinit = [0 0];
% xinit = [0 70];
% xinit = [0 30];
x(1,:) = xinit;
d1 = delta*R*[0 1]';
d2 = delta*R*[cos(pi/3) 1/2]';
x(2,:) = d1'+xinit;
x(3,:) = d2'+xinit;
% 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 = [-1,1];
% rpos = [-1,-1];
% rpos = [0.5,0.5];
waveenv = WaveEnv();
yfun = @(angles,k)waveenv.getIntensityByYP(rpos(1),rpos(2),angles(1),angles(2),k/40);
% 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(100:end,:))
% % divergence value 
% %  223.3858   20.7546 1.625e-7
% % save te x y;
% % 
idx = 1:4:numel(x(:,1))-1;
% figure

plot(idx,env.HisIntensities(1,idx),'-','LineWidth',1.2);
hold on   
plot(idx,y(idx),'--','LineWidth',1.2);

% load int
% plot(int(1,:));
plot(idx,static_intensity(idx),':.','LineWidth',1.2);

% legend('DQN,wind 0 m/s','TE,wind 0 m/s','0 m/s','DQN,wind 10 m/s','TE,wind 10 m/s','10 m/s')

legend('DQN','TE','Without Adjustment')

xlabel( "Step")
ylabel("Intensity (W)")
grid on;
xticks(0:50:200)
% 
% 
figure
idx = 1:4:numel(x(:,1))-1;
a=plot(idx,env.HisAngle(1,idx ),'^-','color',rgb('lavender'),'LineWidth',0.5);
%a.Color(4)=0;
hold on
b=plot(idx,env.HisAngle(2,idx ),'s-','color',rgb('rosy pink'),'LineWidth',0.5);
%b.Color(4)=0;

% plot(idx,env.HisAngle(1,idx )+randn(size(env.HisAngle(1,idx ))),'^-','color',rgb('lavender'))
% hold on
% plot(idx,env.HisAngle(2,idx )+randn(size(env.HisAngle(1,idx ))),'s-','color',rgb('rosy pink'))
c=plot(idx,x(idx ,1),'x-','color',rgb('windows blue'),'LineWidth',0.5);
%c.Color(4)=0;
d=plot(idx,x(idx ,2),'*-','color',rgb('boring green'),'LineWidth',0.5);
%d.Color(4)=0;
ylim([-60 60])
% load angels.mat
% plot(angles(1,:))
% plot(angles(2,:))
xlabel( "Step")
ylabel("Angle (\circ)")
legend("DQN Yaw Angle","DQN Pitch Angle","TE Yaw Angle","TE Pitch Angle")
grid on;
xticks(0:50:200)