427 lines
15 KiB
C#
427 lines
15 KiB
C#
namespace FoodFactory.Conveyors;
|
||
|
||
using Chickensoft.Introspection;
|
||
using Chickensoft.AutoInject;
|
||
using Godot;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using Chickensoft.Sync.Primitives;
|
||
using SJK.Functional;
|
||
using SJK.Math;
|
||
using FoodFactory.Items;
|
||
using FoodFactory.Voxel;
|
||
|
||
[Meta(typeof(IAutoNode))]
|
||
[Tool]
|
||
public partial class TestItemConveyor : Node, IMovementConveyor
|
||
{
|
||
public override void _Notification(int what) => this.Notify(what);
|
||
[Dependency] public IBeltMovement MovementSystem => this.DependOn<IBeltMovement>(() => new IndividualMovement());
|
||
[Dependency] public IVoxelGridRegistry GridRegistry => this.DependOn<IVoxelGridRegistry>();
|
||
[Dependency] public IFoodFactoryApi FoodFactoryApi => this.DependOn<IFoodFactoryApi>();
|
||
// private readonly AutoList<ConveyorSlice> _items = [];
|
||
// public IAutoList<ConveyorSlice> Items => _items;
|
||
// public readonly Sorted1DList<ConveyorSlice> Items = new(pos => pos.BeltT);
|
||
private readonly Ordered1DList<IBeltItem> _items = new();
|
||
public Ordered1DList<IBeltItem> Items => _items;
|
||
// private AutoList<ConveyorSlice>.Binding _itemsBinding;
|
||
public IBeltPort StartPort { get; set; }
|
||
// public IBeltSlotProfile StartPort => new ItemConveyor.ConveyorPort() {
|
||
// Face = Direction.Back,
|
||
// MovementConveyor = this,
|
||
// LocalOffset = Position,
|
||
// BeltT = new ItemConveyor.BeltTEnd(ItemConveyor.ConveyorEnd.Start),
|
||
// PullPush = TransferMode.PushPull ,
|
||
// Direction = PortAccess.BiDirectional,
|
||
// AcceptItemFunc = (item,offset)=>{ _items.Insert(0,new ConveyorSlice(item,offset)); return true;},
|
||
// CanAcceptItemFunc = (item, offset) =>
|
||
// {
|
||
// return .25f <= GetDistanceToNextItem(ItemConveyor.BeltDirection.TowardEnd, 0, 5f, LaneSpan.One);
|
||
// }
|
||
// };
|
||
public IBeltPort EndPort { get; set; }
|
||
// public IBeltSlotProfile EndPort => new ItemConveyor.ConveyorPort() {
|
||
// Face = Direction.Front,
|
||
// MovementConveyor = this,
|
||
// Direction = PortAccess.BiDirectional,
|
||
// LocalOffset = Position,
|
||
// BeltT = new ItemConveyor.BeltTEnd(ItemConveyor.ConveyorEnd.End),
|
||
// PullPush = TransferMode.PushPull,
|
||
// AcceptItemFunc = (item,offset)=>{ _items.Add(new ConveyorSlice(item,Length-offset)); return true;},
|
||
// CanAcceptItemFunc = (item,offset)=> .25f >= GetDistanceToNextItem(ItemConveyor.BeltDirection.TowardStart,Length,.5f,LaneSpan.One)
|
||
// };
|
||
public IList<IBeltPort> OtherPorts = [];
|
||
// [Export] public Vector3I Position { get; set; } = default!;
|
||
//Speed per unit time
|
||
private readonly AutoValue<float> _speed = new(.5f);
|
||
public IAutoValue<float> SpeedValue => _speed;
|
||
|
||
public float SignedSpeed
|
||
{
|
||
get => _speed.Value;
|
||
set => _speed.Value = value;
|
||
}
|
||
|
||
public float SpeedMagnitude
|
||
{
|
||
get => Mathf.Abs(_speed.Value);
|
||
set => _speed.Value = Mathf.Abs(value) * Mathf.Sign(_speed.Value);
|
||
}
|
||
public bool IsReversed { get => _speed.Value < 0; set => _speed.Value = SpeedMagnitude * (value ? -1 : 1); }
|
||
// public IList<ConveyorSlice> Items => _items;
|
||
public float Length { get; set; } = 1;
|
||
|
||
// public IEnumerable<ConveyorSlice> EnumerateTowardEnd() => _items;
|
||
|
||
// public IEnumerable<ConveyorSlice> EnumerateTowardStart() => _items.Reverse();
|
||
public BeltDirection GetBeltDirection() => IsReversed ? BeltDirection.TowardStart : BeltDirection.TowardEnd;
|
||
public IBeltMovement GetMovementPolicy() => MovementSystem;
|
||
public IEnumerable<IBeltPort> GetPorts() => [StartPort, EndPort, .. OtherPorts];
|
||
public void OnResolved()
|
||
{
|
||
if (Engine.IsEditorHint())
|
||
{
|
||
return;
|
||
}
|
||
// var timer = new Timer() { WaitTime = .05f, Autostart = true };
|
||
// AddChild(timer);
|
||
FoodFactoryApi.TickManger.GameTick += args => MovementSystem.AdvanceBelt(this, (float)args.Delta);
|
||
if (StartPort is null || EndPort is null)
|
||
{
|
||
throw new Exception();
|
||
}
|
||
foreach (var item in GetPorts())
|
||
{
|
||
var points = item.Points();
|
||
var guid = GridRegistry.Register(item, [.. points]);
|
||
TreeExiting += () => GridRegistry.UnRegister(guid);
|
||
}
|
||
}
|
||
//ChatGPT Assisted
|
||
public BeltObstacle GetDistanceToNextItem(
|
||
BeltDirection beltDirection,
|
||
float itemBeltT,
|
||
float maxDistToCheck,
|
||
LaneSpan laneSpan,
|
||
HashSet<IMovementConveyor>? visited = null)
|
||
{
|
||
if (beltDirection == BeltDirection.NotMoving)
|
||
{
|
||
throw new NotSupportedException();
|
||
}
|
||
|
||
visited ??= [];
|
||
if (!visited.Add(this))
|
||
{
|
||
return new BeltObstacle(0);
|
||
}
|
||
|
||
bool towardStart = beltDirection == BeltDirection.TowardStart;
|
||
|
||
// 1️⃣ Try to find next item on this conveyor
|
||
var nextItem = FindNextLocalItem(towardStart, itemBeltT, laneSpan);
|
||
// GD.Print(nextItem);
|
||
if (nextItem.HasValue(out var item))
|
||
{
|
||
return new ItemBeltObstacle(MathF.Abs(itemBeltT - item.BeltT) - ITEMSIZE, item, laneSpan);
|
||
// return MathF.Abs(itemBeltT - item.BeltT);//TODO UseLaneSpan inadditon
|
||
}
|
||
|
||
// 2️⃣ No local item → try crossing into adjacent conveyor
|
||
var recursiveDistance = GetDistanceAcrossPort(
|
||
towardStart,
|
||
itemBeltT,
|
||
maxDistToCheck,
|
||
laneSpan,
|
||
visited
|
||
);
|
||
|
||
return recursiveDistance;// with { Distance = recursiveDistance.Distance + Length };
|
||
}
|
||
//ChatGPT Assisted
|
||
private IOption<ConveyorSlice> FindNextLocalItem(
|
||
bool towardStart,
|
||
float itemBeltT,
|
||
LaneSpan laneSpan)
|
||
{
|
||
var sequence = towardStart
|
||
? _items.EnumerateTowardStart(itemBeltT)
|
||
: _items.EnumerateTowardEnd(itemBeltT);
|
||
while (sequence.MoveNext())
|
||
{
|
||
var item = sequence.Current;
|
||
if (towardStart ? item.Position < itemBeltT : item.Position > itemBeltT)
|
||
{
|
||
return Some<ConveyorSlice>.Of(new ConveyorSlice(item.Value, item.Position));
|
||
}
|
||
}
|
||
return None<ConveyorSlice>.Of();
|
||
// return sequence
|
||
// .Where(i => towardStart ? i.Value.BeltT < itemBeltT : i.Value.BeltT > itemBeltT)
|
||
// .Where(i => LaneSpan.OverLaps(i.Value.LaneSpan, laneSpan))
|
||
// .Select(item => item.Value)
|
||
// .FirstOrNone();
|
||
}
|
||
//ChatGPT Assisted
|
||
private BeltObstacle GetDistanceAcrossPort(
|
||
bool towardStart,
|
||
float itemBeltT,
|
||
float maxDistToCheck,
|
||
LaneSpan laneSpan,
|
||
HashSet<IMovementConveyor> visited)
|
||
{
|
||
var port = towardStart ? StartPort : EndPort;
|
||
var boundaryT = towardStart ? 0f : Length;
|
||
var distanceToBoundary = MathF.Abs(itemBeltT - boundaryT);
|
||
|
||
// 🚫 Boundary already exceeds budget
|
||
// if (distanceToBoundary >= maxDistToCheck)
|
||
// {
|
||
// return maxDistToCheck;
|
||
// }
|
||
|
||
|
||
|
||
|
||
var neighborPort = FindFacingConveyorPort(port);
|
||
if (!neighborPort.HasValue(out var conveyorPort))
|
||
{
|
||
var o = GetPortFacing(port);
|
||
if (o.HasValue(out var otherport))
|
||
{
|
||
var mappedLane2 = MapLaneOrFail(port, otherport, laneSpan);
|
||
if (mappedLane2.HasValue(out var lane2))
|
||
{
|
||
return new PortBeltObstacle(distanceToBoundary, lane2, otherport);
|
||
// return new BeltObstacle(ObstacleKind.Boundary,distanceToBoundary+1,lane1,this,null, null);//TODO THIS SHOULD GET SPACE IN PORT SO ITEMS KNOW IF THEY CAN FIT
|
||
|
||
}
|
||
return new BoundaryBeltObstacle(distanceToBoundary);
|
||
// return new BeltObstacle(ObstacleKind.Boundary,distanceToBoundary,laneSpan,this,null, null);//TODO THIS SHOULD GET SPACE IN PORT SO ITEMS KNOW IF THEY CAN FIT
|
||
}
|
||
return new BoundaryBeltObstacle(distanceToBoundary);
|
||
}
|
||
|
||
if (conveyorPort.BeltT is not BeltTEnd end)
|
||
{
|
||
return new BoundaryBeltObstacle(distanceToBoundary);
|
||
}
|
||
|
||
var mappedLane = MapLaneOrFail(port, conveyorPort, laneSpan);
|
||
if (!mappedLane.HasValue(out var lane))
|
||
{
|
||
return new BoundaryBeltObstacle(distanceToBoundary);
|
||
}
|
||
|
||
var nextDirection = DirectionAwayFromEnd(end.End);
|
||
var startT = end.End == ConveyorEnd.Start
|
||
? 0f
|
||
: conveyorPort.Conveyor.Length;
|
||
|
||
// 🔻 Remaining budget after reaching boundary
|
||
// var remainingDist = maxDistToCheck - distanceToBoundary;
|
||
// if (remainingDist <= 0f)
|
||
// {
|
||
// return maxDistToCheck;
|
||
// }
|
||
|
||
var recursiveDistance =
|
||
conveyorPort.Conveyor.GetDistanceToNextItem(
|
||
nextDirection,
|
||
startT,
|
||
maxDistToCheck,
|
||
lane,
|
||
visited
|
||
);
|
||
return recursiveDistance with { Distance = recursiveDistance.Distance + distanceToBoundary };
|
||
}
|
||
private IOption<ConveyorPort> FindFacingConveyorPort(
|
||
IBeltPort slot)
|
||
{
|
||
var targetpos = slot.Profile.LocalOffset.TransformDirection(Vector3I.Forward);
|
||
// var targetPos = port.Profile.Position + port.Profile.Face.ToVector();
|
||
return GridRegistry
|
||
.Get<IBeltPort>(slot.Profile.LocalOffset.LocalToWorld(Vector3I.Forward))
|
||
|
||
.Where(port => port is ConveyorPort)
|
||
.Where(port => port.Profile.LocalOffset.TransformDirection(Vector3I.Forward) == slot.Profile.LocalOffset.TransformDirection(Vector3I.Back))
|
||
.FirstOrNone()
|
||
.Bind(p => p is ConveyorPort cp
|
||
? cp.ToOption()
|
||
: None<ConveyorPort>.Of());
|
||
}
|
||
public IOption<IBeltPort> GetPortFacing(IBeltPort slot) => slot.GetPortFacing(GridRegistry);
|
||
private IOption<LaneSpan> MapLaneOrFail(
|
||
IBeltPort from,
|
||
IBeltPort to,
|
||
LaneSpan incoming)
|
||
{
|
||
// return None<LaneSpan>.Of();
|
||
bool mirror = false;
|
||
// full mapping of the source port
|
||
var mapped = from.MapLaneSpanToFacingPort(to);
|
||
|
||
if (mapped == LaneSpan.Zero)
|
||
return LaneSpan.Zero.ToOption();
|
||
|
||
int sourceWidth = from.Profile.Width;
|
||
int targetWidth = mapped.Width;
|
||
|
||
int startOffset = incoming.Start; // start relative to source port
|
||
int spanLength = incoming.Width;
|
||
|
||
int newStart;
|
||
if (mirror)
|
||
{
|
||
// mirrored: right side of incoming aligns with right side of mapped
|
||
newStart = mapped.End - (startOffset + spanLength);
|
||
}
|
||
else
|
||
{
|
||
// normal: left side of incoming aligns with left side of mapped
|
||
newStart = mapped.Start + startOffset;
|
||
}
|
||
|
||
int newEnd = newStart + spanLength;
|
||
|
||
// clamp to mapped range
|
||
newStart = Math.Max(mapped.Start, newStart);
|
||
newEnd = Math.Min(mapped.End, newEnd);
|
||
|
||
// GD.Print(incoming, new LaneSpan((ushort)newStart, (ushort)newEnd));
|
||
if (newStart >= newEnd)
|
||
{
|
||
// GD.Print("none");
|
||
return None<LaneSpan>.Of();
|
||
}
|
||
|
||
return new LaneSpan((ushort)newStart, (ushort)newEnd).ToOption();
|
||
}
|
||
|
||
private BeltDirection DirectionAwayFromEnd(ConveyorEnd conveyorEnd) => conveyorEnd switch { ConveyorEnd.Start => BeltDirection.TowardEnd, ConveyorEnd.End => BeltDirection.TowardStart, _ => throw new NotSupportedException() };
|
||
// public IEnumerable<Sorted1DList<ConveyorSlice>.ItemHandle> EnumerateTowardEnd() => Items.EnumerateTowardEnd();
|
||
// public IEnumerable<Sorted1DList<ConveyorSlice>.ItemHandle> EnumerateTowardStart() => Items.EnumerateTowardStart();
|
||
private static readonly Vector3[] _square = [new(-.5f, .5f, -.5f), new(.5f, .5f, -.5f), new(.5f, -.5f, -.5f), new(-.5f, -.5f, -.5f), new(-.5f, .5f, -.5f)];
|
||
public const float ITEMSIZE = .2f;
|
||
private const float ITEMHALFSIZE = ITEMSIZE / 2f;
|
||
|
||
// public override void _Process(double delta)
|
||
// {
|
||
// if (StartPort is null || EndPort is null)
|
||
// {
|
||
// return;
|
||
// }
|
||
// foreach (var item in GetPorts())
|
||
// {
|
||
// for (int ii = 0; ii < item.Profile.Width; ii++)
|
||
// {
|
||
// DebugDraw3D.DrawLinePath(_square.Select(i => item.Profile.LocalOffset.ToGodot().TranslatedLocal(i).TranslatedLocal(new Vector3(ii, 0, 0)).Origin).ToArray(), (!Engine.IsEditorHint()) && GetPortFacing(item).HasValue() ? Colors.Green : Colors.Red);
|
||
|
||
// }
|
||
// // DebugDraw3D.DrawArrow(item.Profile.LocalOffset.Origin, item.Profile.LocalOffset.LocalToWorld(item.Profile.Face.ToVector()),(!Engine.IsEditorHint())&&GetPortFacing(item).HasValue()?Colors.Green:Colors.Red);
|
||
// }
|
||
// }
|
||
|
||
public ConveyorPort CreatePort(BeltPortProfile profile, BeltT beltT, LaneSpan laneSpan)
|
||
{
|
||
// var towardStartEnd = beltT is ItemConveyor.BeltTEnd end && end.End == ItemConveyor.ConveyorEnd.End;
|
||
bool accept(IBeltItem item, float offset, LaneSpan lane)
|
||
{
|
||
if (beltT is BeltTEnd end)
|
||
{
|
||
var startBeltT = end.End == ConveyorEnd.Start ? 0 : Length;
|
||
var obstacle = GetDistanceToNextItem(end.End.Swap().DirectionTo(), startBeltT, offset, lane);
|
||
var distanceAllowed = obstacle.Distance;
|
||
return ITEMSIZE < distanceAllowed;
|
||
}
|
||
else if (beltT is BeltTOffset endOffset)
|
||
{
|
||
return HasClearance(endOffset.T, lane);
|
||
}
|
||
throw new NotImplementedException();
|
||
}
|
||
bool tryInsert(IBeltItem item, float offset, LaneSpan laneSpan)
|
||
{
|
||
if (beltT is BeltTEnd end)
|
||
{
|
||
if (end.End == ConveyorEnd.End)
|
||
{
|
||
_items.Insert(item, Length - offset);
|
||
// Items.Insert(new(item, Length - offset) { LaneSpan = laneSpan });
|
||
// _items.Add(new(item, Length-offset));
|
||
}
|
||
else
|
||
{
|
||
_items.Insert(item, offset);
|
||
// Items.Insert(new(item, offset) { LaneSpan = laneSpan });
|
||
// _items.Insert(0, new(item, offset));
|
||
}
|
||
return true;
|
||
}
|
||
else if (beltT is BeltTOffset endOffset)
|
||
{
|
||
|
||
if (!HasClearance(endOffset.T, LaneSpan.One))
|
||
{
|
||
return false;
|
||
}
|
||
_items.Insert(item, endOffset.T);
|
||
// Items.Insert(new(item, endOffset.T) { LaneSpan = laneSpan });
|
||
// int insertIndex = FindInsertIndex(endOffset.T);
|
||
|
||
// _items.Insert(insertIndex, new(item, endOffset.T));
|
||
|
||
return true;
|
||
}
|
||
// _items.Insert(0,new ConveyorSlice(item,0));
|
||
// return true;
|
||
throw new NotImplementedException();
|
||
}
|
||
|
||
var port = new ConveyorPort(this,
|
||
profile,
|
||
beltT,
|
||
accept,
|
||
tryInsert);
|
||
return port;
|
||
}
|
||
private bool HasClearance(float centerT, LaneSpan span)
|
||
{
|
||
var lower = GetDistanceToNextItem(
|
||
BeltDirection.TowardStart,
|
||
centerT,
|
||
ITEMSIZE,
|
||
span
|
||
);
|
||
|
||
var upper = GetDistanceToNextItem(
|
||
BeltDirection.TowardEnd,
|
||
centerT,
|
||
ITEMSIZE,
|
||
span
|
||
);
|
||
|
||
float lowerAllowed =
|
||
lower.Distance;// -
|
||
// (lower.IsItem ? ItemConveyor.ITEMSIZE : 0);
|
||
|
||
float upperAllowed =
|
||
upper.Distance;// -
|
||
// (upper.IsItem ? ItemConveyor.ITEMSIZE : 0);
|
||
|
||
return ITEMSIZE < lowerAllowed &&
|
||
ITEMSIZE < upperAllowed;
|
||
}
|
||
|
||
public Ordered1DList<IBeltItem>.Enumerator EnumerateTowardEnd() => _items.EnumerateTowardEnd(-1);
|
||
public Ordered1DList<IBeltItem>.Enumerator EnumerateTowardStart() => _items.EnumerateTowardStart(_items.Count);
|
||
public bool HasItems() => _items.Count > 0;
|
||
}
|
||
//TODO Should liklely account for max search distance where the conveyorm may be needed to know
|
||
public record class BeltObstacle(float Distance);
|
||
public record ItemBeltObstacle(float Distance, ConveyorSlice Item, LaneSpan LaneSpan) : BeltObstacle(Distance);
|
||
public record BoundaryBeltObstacle(float DistanceToBoundary) : BeltObstacle(DistanceToBoundary);
|
||
public record PortBeltObstacle(float DistanceToBoundary, LaneSpan LaneSpan, IBeltPort BeltPort) : BoundaryBeltObstacle(DistanceToBoundary);
|
||
|