% ============================================================================
% GetThrustForDescent.m
% Author: Brent Dingle, Ph.D.
% Creation Date: 2020
%
% Note, if we run out of fuel then what we calculate here won't matter
% as there will be no fuel to create thrust
%
% See also InitLandingController1.m for explanation of some details
%
% TODO: There is also likley a maximum thrust a given engine 
%       could physically achieve
%
% TODO: Scrap all this and use a target velocity based 
%       on distance from target height.
%       No modes, just always checking distance 
%       (or change in distance from previous step?)
% ============================================================================
function guide = GetThrustForDescent(sim, rock, ctrl)

  % Init return value to zero
  thrust = 0;
  guide.farCorrectionEnabled = false;
  
  % The closer we are to target height the more we want velocity to be zero
  % We assume we are falling towards that desired height (we start above it)
  % How far are we from hover height?
  errorHeight = rock.height - ctrl.heightDesired; 
  
  % The distances used here depend on maximum thrust and mass
  % They are landing control parameters
  
  % ----------------------------------------------------------------------
  % If we are 'far' away, let gravity pull us down -up to a max velocity
  % We then correct to a desired velocity and then allow free fall again
  % Idea is to constrain ourselves to a range of velocity
  % ----------------------------------------------------------------------
  if (errorHeight >= ctrl.farDistance)
    % As long as we don't exceed a maximum velocity, we keep free falling
    thrust = 0;  % stay in free fall
    
    % These velocities are non-positive numbers for downward direction
    % So comparison signs may seem backwards
    if (rock.vel < ctrl.farVelMax)
      ctrl.farCorrectionEnabled = true;
    endif
    
    if ( (ctrl.farCorrectionEnabled == true) && (rock.vel < ctrl.farVelTarget) )
      errorVel = ctrl.farVelTarget - rock.vel; % small neg - big neg = positive number
      % First adjust to match our desired velocity
      % if gain == 1, we are aggressively matching to our desired velocity
      % if gain == 0, we are not trying at all
      nThrust = ctrl.farGain * errorVel;
      
      % Note, if error is 0 then we still have to adjust for gravity
      % So we add enough thrust to counteract gravity too
      % This achieves a 'constant' velocity by canceling acceleration from gravity
      g = GetGravity(sim, rock.height);
      totalMass = rock.baseMass + rock.fuelMass;
      thrust = nThrust + totalMass * g;  % return value is a Force so add F=mg  to thrust
      %thrust = 0.9 * thrust; % this is to ensure we don't oscillate in corrections here -- deliberate fail to correct gravity
      
      if (errorVel < 0.5)  % within 0.5 m/s of target velocity
        ctrl.farCorrectionEnabled = false;
      endif
      
    endif
    
  % ----------------------------------------------------------------------
  % Else if we are >= medDistance and < farDistance
  % Then we must start slowing down, if we have not already
  % ----------------------------------------------------------------------
  elseif (errorHeight >= ctrl.medDistance)
      errorVel = ctrl.medVelocity - rock.vel; % small neg - big neg = positive number
      nThrust = ctrl.medGain * errorVel;
      
      % Note, if error is 0 then we still have to adjust for gravity
      % So we add enough thrust to counteract gravity too
      % This achieves a 'constant' velocity by canceling acceleration from gravity
      g = GetGravity(sim, rock.height);
      totalMass = rock.baseMass + rock.fuelMass;
      thrust = nThrust + totalMass * g;  % return value is a Force so add F=mg  to thrust
%      thrust = 0.9 * thrust; % -- deliberate fail to correct gravity
    
  % ----------------------------------------------------------------------
  % Else if we are >= shortDistance and < medDistance
  % Then we must start slowing down, if we have not already
  % ----------------------------------------------------------------------
  elseif (errorHeight >= ctrl.shortDistance)
      errorVel = ctrl.shortVelocity - rock.vel; % small neg - big neg = positive number
      nThrust = ctrl.shortGain * errorVel;
      
      % Note, if error is 0 then we still have to adjust for gravity
      % So we add enough thrust to counteract gravity too
      % This achieves a 'constant' velocity by canceling acceleration from gravity
      g = GetGravity(sim, rock.height);
      totalMass = rock.baseMass + rock.fuelMass;
      thrust = nThrust + totalMass * g;  % return value is a Force so add F=mg  to thrust
    
  % ----------------------------------------------------------------------
  % Else if we are >= smallDistance and < shortDistance
  % Then we must continue slowing down
  % ----------------------------------------------------------------------
  elseif (errorHeight >= ctrl.smallDistance)
      errorVel = ctrl.smallVelocity - rock.vel; % small neg - big neg = positive number
      nThrust = ctrl.smallGain * errorVel;
      
      % Note, if error is 0 then we still have to adjust for gravity
      % So we add enough thrust to counteract gravity too
      % This achieves a 'constant' velocity by canceling acceleration from gravity
      g = GetGravity(sim, rock.height);
      totalMass = rock.baseMass + rock.fuelMass;
      thrust = nThrust + totalMass * g;  % return value is a Force so add F=mg  to thrust
    
  % ----------------------------------------------------------------------
  % Else if we are >= hoverDistance and < smallDistance
  % Then we must be very near or hover velocity (likely 0)
  % ----------------------------------------------------------------------
  else  %if (errorHeight >= ctrl.hoverDistance)
      errorVel = ctrl.hoverVelocity - rock.vel; % small neg - big neg = positive number
      nThrust = ctrl.hoverGain * errorVel;
      
      % Note, if error is 0 then we still have to adjust for gravity
      % So we add enough thrust to counteract gravity too
      % This achieves a 'constant' velocity by canceling acceleration from gravity
      g = GetGravity(sim, rock.height);
      totalMass = rock.baseMass + rock.fuelMass;
      thrust = nThrust + totalMass * g;  % return value is a Force so add F=mg  to thrust
 
  endif
  
  
  % TODO: check if thrust has exceeded a maxiumum possible for current engine
  %       if (thrust > max) then thrust = max 
  
  % guide structure is used to pass back multiple return values
  guide.thrust = thrust;
  guide.farCorrectionEnabled = ctrl.farCorrectionEnabled;
  %guide.isSlowing = rock.isSlowing;
  %guide.slowStart = rock.slowStart;
  
endfunction