classdef DiscreteLightIntensityEnv < rl.env.MATLABEnvironment
    %LIGHTINTENSITYENV: Template for defining custom environment in MATLAB.    
    
    %% Properties (set properties' attributes accordingly)
    properties        
        %接收平面初始水平转动的角为 0度
        YawAngle= 0;
        %接收平面初始俯仰角为 0 度
        PitchAngle = 30;
        %接收平面的的法向量量初始时竖直向下
        n = [0 0 -1]';
        %   UAV位置
        rpos = 0;
        % 接收器最低光强
        IntensityThreshold = 10^(-10);    
        %当前时间
        t = 0;
        % 采样时间
        Ts = 0.02;
        Scale = 2;
        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 = DiscreteLightIntensityEnv(rpos,sim)
            
           % Initialize Observation settings.
            numObs = 4;
            ObservationInfo = rlNumericSpec([numObs 1]);
            ObservationInfo.Name = 'observation';
            %给出造成结果的原因
            ObservationInfo.Description = 'rpos(1) rpos(2)  pitch yaw ';
            
            % Initialize Action settings  
%             numAct = 9;         
%             ActionInfo = rlFiniteSetSpec(num2cell(1:numAct));
            % 每次移动角度大小
            ActionInfo = rlFiniteSetSpec({ ...
                [-1 -1],[-1  0],[-1 1], ...
                [ 0 -1],[ 0  0],[ 0 1], ...
                [ 1 -1],[ 1  0],[ 1 1]});
            ActionInfo.Name = 'action';
            % 俯仰角和转向角的增量
            ActionInfo.Description = 'delta yaw, delta pitch';
            % 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,action)
            LoggedSignals = [];
            if this.sim
                this.HisAngle = [this.HisAngle [this.YawAngle;this.PitchAngle]];
            end
            act = modifyDelta(this,action);

            dy = act(1);
            dp = act(2);

            yaw = this.YawAngle + dy;
            pitch = this.PitchAngle + dp;

            if pitch > 180
              pitch = 180;
            elseif pitch <-180 
              pitch = -180;
            end
            if yaw >360
              yaw = 360;
            elseif yaw <0
              yaw = 0;
            end

%             this.t  =this.t +0.01;
            %接收到的光强
            intensity = this.wave.getIntensityByYP(this.rpos(1),this.rpos(2),yaw,pitch,this.t);
            
            % Update system states
            this.PitchAngle = pitch;
            this.YawAngle = yaw;
            
            Observation = [this.rpos(1);this.rpos(2);this.PitchAngle;this.YawAngle];
            this.State = Observation;
           
            % Check terminal condition
            IsDone = length(this.HisIntensities) > 200 || intensity <= this.IntensityThreshold;
%             IsDone = length(this.HisIntensities) > 200 ;
            this.IsDone = IsDone;
           
            this.HisIntensities = [this.HisIntensities,intensity];
            % Get reward
            Reward = getReward(this);
            notifyEnvUpdated(this);
        end
        
        % Reset environment to initial state and output initial observation
        function InitialObservation = reset(this)
%             "reset env"   ，测试每次是否调用reset函数
            this.IsDone = false;
            this.YawAngle= 0;
            this.PitchAngle = 30;
            this.HisIntensities = [];
            this.HisReward = [];
            
            intensity = this.wave.getIntensityByYP(this.rpos(1),this.rpos(2),this.YawAngle ,this.PitchAngle,this.t); 
            InitialObservation = [this.rpos(1);this.rpos(2);this.PitchAngle;this.YawAngle];
            this.State = InitialObservation;
            this.HisIntensities = [this.HisIntensities,intensity];
            notifyEnvUpdated(this);
        end
    end
    %% Optional Methods (set methods' attributes accordingly)
    methods               
        % Helper methods to create the environment
        % Discrete force 1 or 2
        function act = modifyDelta(this,action)
            %可以通过调节this.Scale参数调整每一次，角度调整的大小
            act = action*this.Scale;           
        end
        
        % Reward function
        function Reward = getReward(this)
            
            if this.IsDone  
                Reward = 0;
                return
             end  
            
            len = size(this.HisIntensities,2);
            Tc = 1;
            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;
            this.HisReward = [this.HisReward;ir,sr];   
        end
        
        % (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