unit ulazformation;

{$mode delphi}

interface

uses
  Classes, SysUtils, Math,
  uLazFormationGlobals, uHCTypeDefs, uHCGCI, uHCDefines, uHCUtils;

procedure UnitFormationListClear;
procedure GroupOffsetCalculate(pUnit: PHCUNIT);
procedure FormationUnitTurn(pUnit: PHCUNIT);
procedure SetCourseUnit(pUnit: PHCUnit; pCourse: short);
procedure SetCourseGroup(pGroup: PHCGroup; pCourse: short);

implementation


procedure UnitFormationListClear;
var
  lFormation: THCFORMATIONC;
  lUUID: HCUUID;
  lC: integer;
begin
  for lUUID in UnitFormationList.Keys do
  begin
    UnitFormationList.Items[lUUID].Free;
  end;
  UnitFormationList.Clear;
end;

procedure GroupOffsetCalculate(pUnit: PHCUNIT);
var
  lFormation: THCFORMATIONC;
  lRange: double;
  lBearing: double;
  lRangeDU: long;
  lBearingSliver: long;
begin
  if (UnitFormationList.TryGetValue(pUnit^.UUID, lFormation)) then
  begin

    HCFindRangeBearing(pUnit^.Group^.sLat, pUnit^.Group^.sLon, pUnit^.sLat,
      pUnit^.sLon, lRangeDU, lBearingSliver);

    lRange := lRangeDU / NM1;
    lBearing := lBearingSliver / SLIVER1;

    lFormation.PositionNow.Range := lRange;
    lFormation.PositionNow.Bearing := lBearing;
    UnitFormationList.AddOrSetValue(pUnit^.UUID, lFormation);
  end
  else
  begin
    lRangeDU := -1;
    lBearingSliver := -1;
  end;
end;

procedure FormationUnitTurn(pUnit: PHCUNIT);
var
  lFormation: THCFORMATIONC;
  lRange: double;
  lRangeGoal: double;
  lBearing: double;
  lBearingGoal: double;
  lGrpLat, lGrpLon: double;
  lUnitLat, lUnitLon: double;
  lUnitLatGoal, lUnitLonGoal: double;
  lRings: ULONGLONG;
  lSectors: ULONGLONG;
  lSectorDegStart, lSectorDegEnd: short;
  lSectorCount: byte;
  lZoneGoal: short;
  lITime: long;
  lIDistance, lIAngle: double;
begin
  if (UnitFormationList.TryGetValue(pUnit^.UUID, lFormation)) then
  begin
    lGrpLat := SliverToDegLatF(pUnit^.Group^.sLat);
    lGrpLon := SliverToDegLonF(pUnit^.Group^.sLon);
    lUnitLat := SliverToDegLatF(pUnit^.sLat);
    lUnitLon := SliverToDegLonF(pUnit^.sLon);

    // ALE 20180313 Figure out where we are now compared to group center
    //  could also be done by calling GroupOffsetCalculate
    HCGetRangeBearing(lGrpLat, lGrpLon, lUnitLat, lUnitLon, lRange, lBearing);
    lFormation.PositionNow.Range := lRange;
    lFormation.PositionNow.Bearing := lBearing;

    // ALE 20180313 Figure out where we'd like to be
    //  1. If main body, use unit^.xOff, unit^.yOff
    //  2. If outside of main body, first pass is to place
    //      in center of first sector we find from lRings, lSectors
    lRings := PUnit^.rings;
    lSectors := PUnit^.sectors;

    if ((lRings and MainBodyFlag) > 0) then
    begin
      // ALE 20180313 Figure out where we'd like to be
      //  1. If main body, use unit^.xOff, unit^.yOff
      lUnitLatGoal := SliverToDegLatF(pUnit^.Group^.sLat + lFormation.PositionGoal.yOff {pUnit^.yOff});
      lUnitLonGoal := SliverToDegLonF(pUnit^.Group^.sLon + lFormation.PositionGoal.xOff {pUnit^.xOff});
      HCGetRangeBearing(lGrpLat, lGrpLon, lUnitLatGoal, lUnitLonGoal, lRangeGoal, lBearingGoal);
    end
    else
    begin
      // ALE 20180313 Figure out where we'd like to be
      //  2. If outside of main body, first pass is to place
      //      in center of middle sector we find from lRings, first lSectors
      lSectorCount := HCFormationSectorCount(lSectors);
      HCFormationSectors2Deg(lSectors, lSectorDegStart, lSectorDegEnd);
      lBearingGoal := lSectorDegStart + (45 * (lSectorCount / 2.0));
      if (lBearingGoal > 360) then
         lBearingGoal := lBearingGoal - 360;
      lZoneGoal := HCFormationZoneInner(lRings);
      lRangeGoal := pUnit^.Group^.zones[lZoneGoal-1]
       + (pUnit^.Group^.zones[lZoneGoal] - pUnit^.Group^.zones[lZoneGoal-1])/2;
      Polar2DestLatLon(lGrpLat, lGrpLon, lBearingGoal, lRangeGoal, lUnitLatGoal, lUnitLonGoal);
    end;
    // ALE 20180410 assign out result to our formation goal position
    lFormation.PositionGoal.Range := lRangeGoal;
    lFormation.PositionGoal.Bearing := lBearingGoal;

    if (not SameValue(lRangeGoal, 0, 0.01)) then
    begin
      // ALE 20180410 how should we get to the destination?
      //  See if intercept is possible
      //  In testing, another concept for the future is to consider slowing down
      //   the unit if it already ran past its goal position
      while (HCInterceptAngleDeg(lUnitLat, lUnitLon, lUnitLatGoal, lUnitLonGoal,
       pUnit^.kts, pUnit^.Group^.kts, pUnit^.Group^.course, lITime, lIDistance,
       lIAngle) = CFALSE) do
      begin
        // ALE 20180410 try speeding up the unit to see if it can intercept in a reasonable time, if not, consider
        //  slowing the group so that the unit can reform
        if (pUnit^.kts < pUnit^.maxspeed) then
        begin
          Inc(pUnit^.kts);
          pUnit^.speed := pUnit^.kts * NM1;
        end
        else
        begin
          break;
        end;
      end;
      if (SameValue(lIDistance, 0, 0.01)) then
      begin
        // ALE 20180411 destination reached, re-align
        SetCourseUnit(pUnit, pUnit^.Group^.course);
        if (pUnit^.kts <> pUnit^.Group^.kts) then
        begin
          pUnit^.kts := pUnit^.Group^.kts;
          pUnit^.speed := pUnit^.kts * NM1;
        end;
      end
      else
      begin
        // ALE 20180411 keep adjusting towards our formation position
        SetCourseUnit(pUnit, Round(lIAngle));
      end;
    end
    else
    begin
      // ALE 20180411 destination reached, re-align
      SetCourseUnit(pUnit, pUnit^.Group^.course);
      if (pUnit^.kts <> pUnit^.Group^.kts) then
      begin
        pUnit^.kts := pUnit^.Group^.kts;
        pUnit^.speed := pUnit^.kts * NM1;
      end;
    end;
    //Polar2DestLatLon(pUnit^.Group^.sLat, pUnit^.Group^.sLon,

    UnitFormationList.AddOrSetValue(pUnit^.UUID, lFormation);
  end
  else
  begin
    lRange := -1;
    lBearing := -1;
  end;
end;

procedure SetCourseUnit(pUnit: PHCUnit; pCourse: short);
begin
  HCSetCourseUnit_Raw(pUnit, pCourse, 'lazFormation.dll', '', 0);
end;

procedure SetCourseGroup(pGroup: PHCGroup; pCourse: short);
begin
  HCSetCourseGroup_Raw(pGroup, pCourse, 'lazFormation.dll', '', 0);
end;

end.