classdef WaveEnvConAction < rl.env.MATLABEnvironment
    %LIGHTINTENSITYENV: Template for defining custom environment in MATLAB.

    %% Properties (set properties' attributes accordingly)
    properties
        %         两个角度的范围应该是 -180 ~ 180 度之间
        %         接收平面水平转动的角
        YawAngle= 0;
        %         接收平面俯仰角
        PitchAngle = 0;
        %         接收平面的的法向量量初始时竖直向下
        n = [0 0 -1]';
        %           UAV位置
        rpos = 0;
        %         接收器最低光强
        IntensityThreshold = 10^(-10);
        %         当前时间
        t = 0;
        %         采样时间
        Ts = 0.02;
        %         动作——调整大小
%         Scale = 2;
%         Scale = 5;
%         Scale = 4;
        Scale = 7;
        %Scale 越大训练出的角度越容易稳定，而强度波动也会变大
        %         波浪环境
        wave = 0;
        %         当前是否为测试
        sim = false;
        %         历史状态集
        HisIntensities = [];
        HisReward = [];
        HisAction = [];
        HisAngle = [];
    end

    properties
        % 初始化系统状态
        State = zeros(4,1)
    end

    properties(Access = protected)
        % Initialize internal flag to indicate episode termination
        IsDone = false
    end
    %     1.8474e-7，最大值

    %% Necessary Methods
    methods
        function this = WaveEnvConAction(rpos,sim)
            % Initialize Observation settings.
            numObs = 4;
            ObservationInfo = rlNumericSpec([numObs 1]);
            ObservationInfo.Name = 'observations';
            % 给出造成结果的原因
            % 针对固定位置处的 cos pitch, sin pitch ,cos yaw ,sin yaw
            ObservationInfo.Description = 'cos pitch, sin pitch ,cos yaw ,sin yaw';
            % 每次移动角度大小
            numActions = 2;
            ActionInfo = rlNumericSpec([numActions 1]);;
            ActionInfo.Name = 'action';
            % 俯仰角和转向角的增量
            ActionInfo.Description = 'delta pitch, delta yaw';
            % The following line implements built-in functions of RL env
            this = this@rl.env.MATLABEnvironment(ObservationInfo,ActionInfo);
            this.rpos = rpos;
            this.wave = WaveEnv();
            this.sim = sim;
        end

        % Apply system dynamics and simulates the environment with the
        % given action for one step. action is 2x1 matrix
        function [Observation,Reward,IsDone,LoggedSignals] = step(this,act)
            %当前为测试时，保存历史角度
            if this.sim
                this.HisAngle = [this.HisAngle [this.YawAngle;this.PitchAngle]];
            end
            LoggedSignals = [];
            act = double(act);
            action = double(act)*this.Scale;
            deltaPitch = action(1);
            delatYaw = action(2);

            pitch = this.PitchAngle + deltaPitch;
            yaw = this.YawAngle + delatYaw;
            this.t  =this.t +1/40;

            %接收到的光强
            intensity = this.wave.getIntensityByYP(this.rpos(1),this.rpos(2),yaw,pitch,this.t);

            % Update system states
            
            this.PitchAngle = pitch ;
            this.YawAngle = yaw;

            Observation = [cosd(this.PitchAngle);sind(this.PitchAngle);cosd(this.YawAngle);sind(this.YawAngle)];
            this.State = Observation;

            % Check terminal condition
            IsDone = length(this.HisIntensities) > 200  || intensity<this.IntensityThreshold;
            this.HisIntensities = [this.HisIntensities,intensity];

            len = size(this.HisIntensities,2);
            Tc = 3;
            if len <= Tc                
                prestate = this.HisIntensities(1,:);
            else
                prestate = this.HisIntensities(1,end-Tc:end-1);
            end
            prestate = mean(prestate);
            curstate = this.HisIntensities(1,end);
            %             ir = -(log10(curstate)+6)^2;
            %             ir = 0.5*exp(log10(curstate)+6.8); % 探索到最大值
            ir = log10(curstate)+6.8;
            %             ir = log10(curstate/prestate);
            %             ir = curstate;
            %             ir = 0;
            sr = log(curstate/prestate);
            %             sr = log10(curstate)+6.8;
%             Reward = 0.5*ir+sr;
            Reward = 1.5*ir+sr-0.2*norm(act);
            Reward = 1.1*ir+sr-0.6*norm(act);
            this.HisReward = [this.HisReward;ir,sr]; 
            
            
            notifyEnvUpdated(this);
        end

        % Reset environment to initial state and output initial observation
        function InitialObservation = reset(this)
            %             "reset env"   ，测试每次是否调用reset函数
            this.t = 0;
            this.IsDone = false;
            this.YawAngle= 0;
            this.PitchAngle = 70;
            this.HisIntensities = [];
            this.HisReward = [];

            intensity = this.wave.getIntensityByYP(this.rpos(1),this.rpos(2),this.YawAngle ,this.PitchAngle,this.t);
            InitialObservation =  [cosd(this.PitchAngle);sind(this.PitchAngle);cosd(this.YawAngle);sind(this.YawAngle)];
            this.State = InitialObservation;
            this.HisIntensities = [this.HisIntensities,intensity];
            notifyEnvUpdated(this);
        end
    end
    %% Optional Methods (set methods' attributes accordingly)
    methods

        % (optional) Visualization method
        function plot(this)
            % Initiate the visualization

            % Update the visualization
            envUpdatedCallback(this)
        end
        % (optional) Properties validation through set methods
        function set.State(this,state)
            validateattributes(state,{'numeric'},{'finite','real','vector','numel',4},'','State');
            this.State = state(:);
            notifyEnvUpdated(this);
        end
        function set.PitchAngle(this,val)
            validateattributes(val,{'numeric'},{'finite','real','scalar'},'','PitchAngle');
            this.PitchAngle = val;
            notifyEnvUpdated(this);
        end
        function set.YawAngle(this,val)
            validateattributes(val,{'numeric'},{'finite','real','scalar'},'','YawAngle');
            this.YawAngle = val;
            notifyEnvUpdated(this);
        end
    end

    methods (Access = protected)
        % (optional) update visualization everytime the environment is updated
        % (notifyEnvUpdated is called)
        function envUpdatedCallback(this)
        end
    end
end