unit LocateChargingStation;

interface
uses
  Definitions
  , Behaviors
  , ObjectDefinition
  , Variables
  , Motors
  , Matrices
  , Sensors             { TSensors }
  , Hormones            { THormones }
  , IRDetector          { TIRDetector }
  ;

type
  TLocateChargingStation = class(TMotorBehavior)
  private
    fTurnMotorOutput: real;
    fApproachMotorOutput: real;
    fStopDistance: real;
    fBeaconFound: Boolean;
    fIRDetectorPointer: TIRDetector;
  public
    constructor Create; override;
    constructor CreateAndSet(B: TBehavior); override;
    destructor Destroy; override;
    function Copy: TBehavior; override;
    procedure Initialize(Sensors: TSensors; Hormones: THormones); override;
    procedure LoadFromDefinition(ObjDef: TObjectDefinition); override;
    procedure Step(TimeStep: real); override;
    property TurnMotorOutput: real read fTurnMotorOutput;
    property ApproachMotorOutput: real read fApproachMotorOutput;
    property StopDistance: real  read fStopDistance;
 end;

implementation
uses
  Classes        { RegisterClass, UnRegisterClass }
  ;

////////////////////////////////////////////////////////////////////////////////
{ TLocateChargingStation }
////////////////////////////////////////////////////////////////////////////////
function TLocateChargingStation.Copy: TBehavior;
begin
  Result := TLocateChargingStation.CreateAndSet(Self);
end;

constructor TLocateChargingStation.Create;
begin
  inherited;
  fOutputVariables.SetSize(2);
end;

constructor TLocateChargingStation.CreateAndSet(B: TBehavior);
var
  LC: TLocateChargingStation;
begin
  inherited;
  LC := TLocateChargingStation(B);
  fTurnMotorOutput := LC.TurnMotorOutput;
  fApproachMotorOutput := LC.ApproachMotorOutput;
  fStopDistance := LC.StopDistance;
end;

destructor TLocateChargingStation.Destroy;
begin
  inherited;
end;

procedure TLocateChargingStation.Initialize(Sensors: TSensors;
  Hormones: THormones);
begin
  inherited;

  fIRDetectorPointer :=
    TIRDetector(TSensorVariable(fInputVariables.ItemNamed[
    'IRDetectorInputVariable']).SensorPointer);

end;

procedure TLocateChargingStation.LoadFromDefinition(ObjDef: TObjectDefinition);
begin
  inherited;
  fTurnMotorOutput := ObjDef.PropertyAsFloat('TurnMotorOutput');
  fApproachMotorOutput := ObjDef.PropertyAsFloat('ApproachMotorOutput');
  fStopDistance := ObjDef.PropertyAsFloat('StopDistance');
end;

procedure TLocateChargingStation.Step(TimeStep: real);
var
  AngleToBeacon: real;
begin
  inherited;

  Assert(fIRDetectorPointer <> nil, 'IR detector not available in TLocateChargingStation!');

  AngleToBeacon := fIRDetectorPointer.RelativeAngleToClosestBeacon;

  if (Abs(AngleToBeacon) <= fIRDetectorPointer.OpeningAngle/2) then
    fBeaconFound := True
  else
    fBeaconFound := False;

  if NOT fBeaconFound then
  begin
    if AngleToBeacon > 0 then
    begin
      fOutputVariables[1] := -fTurnMotorOutput;
      fOutputVariables[2] := fTurnMotorOutput;
    end
    else
    begin
      fOutputVariables[1] := fTurnMotorOutput;
      fOutputVariables[2] := -fTurnMotorOutput;
    end;
  end;

  if fBeaconFound then
  begin
    fOutputVariables[1] := fApproachMotorOutput;
    fOutputVariables[2] := fApproachMotorOutput;
  end;

  if fIRDetectorPointer.DistanceToTarget <= fStopDistance then
  begin
    fOutputVariables[1] := 0.0;
    fOutputVariables[2] := 0.0;
  end;
end;

initialization
  RegisterClass(TLocateChargingStation);

finalization
  UnRegisterClass(TLocateChargingStation);

end.