private List <BuildBlock> CreateBlocks()
{
// Group entries by blob
var entriesByBlob = new Dictionary <BuildBlob, List <BuildEntry> >();
foreach (var entry in _table)
{
List <BuildEntry> entries;
if (!entriesByBlob.TryGetValue(entry.Blob, out entries))
{
entries = new List <BuildEntry>();
entriesByBlob.Add(entry.Blob, entries);
}
entries.Add(entry);
}
// Create blocks
var blocks = new List <BuildBlock>();
foreach (var kvp in entriesByBlob)
{
var entries = kvp.Value;
// Sort entries by offset
entries.Sort(delegate(BuildEntry entry1, BuildEntry entry2)
{
return(Comparer <int> .Default.Compare(entry1.Offset, entry2.Offset));
});
BuildBlock block = null;
foreach (var entry in entries)
{
if (block == null || (block.Offset + PageSize) <= entry.Offset)
{
block = new BuildBlock();
block.Blob = entry.Blob;
block.Offset = ((entry.Offset / PageSize) * PageSize);
blocks.Add(block);
}
entry.Offset -= block.Offset;
block.Entries.Add(entry);
}
}
return(blocks);
}
private void Start()
{
var idle = new Idle(this);
var moveToSupply = new MoveToSupply(this);
var moveToSupplyWait = new MoveToSupplyWait(this);
var moveToTarget = new MoveToTarget(this);
var moveToDock = new MoveToDock(this);
var requestTarget = new RequestTarget(this, manager);
var descendToTarget = new DescendToTarget(this);
var ascendFromTarget = new AscendFromTarget(this);
var descendToSupply = new DescendToSupply(this);
var descendToSupplyWait = new DescendToSupplyWait(this);
var ascendFromSupply = new AscendFromSupply(this);
var descendToDock = new DescendToDock(this);
var ascendToTransHeight = new AscendToTransHeight(this);
var waitAtSupply = new WaitAtSupply(this);
var alignToSupply = new AlignToSupply(this);
var alignToSupplyWait = new AlignToSupplyWait(this);
var alignToDock = new AlignToDock(this);
var alignToTarget = new AlignToTarget(this);
var resupply = new Resupply(this);
var buildBlock = new BuildBlock(this);
var navigateToDock = new NavigateToDock(this);
var navigateToSupply = new NavigateToSupply(this);
var navigateToSupplyWait = new NavigateToSupplyWait(this);
var navigateToTarget = new NavigateToTarget(this);
_stateMachine.AddTransition(idle, requestTarget, IsRunning());
_stateMachine.AddTransition(requestTarget, ascendToTransHeight, AssignedTarget());
_stateMachine.AddTransition(requestTarget, navigateToDock, WaitForTarget());
_stateMachine.AddTransition(navigateToDock, moveToDock, ApproxNaviTargetPos);
_stateMachine.AddTransition(moveToDock, alignToDock, ReachedDockXZ());
_stateMachine.AddTransition(alignToDock, descendToDock, ReachedNaviTargetRot);
_stateMachine.AddTransition(descendToDock, idle, ReachedNaviTargetPos);
_stateMachine.AddTransition(ascendToTransHeight, navigateToSupply, ReachedNaviPosSupplyIsCurrent());
_stateMachine.AddTransition(ascendToTransHeight, navigateToSupplyWait, ReachedNaviPosSupplyIsWait());
_stateMachine.AddTransition(navigateToSupply, moveToSupply, ApproxNaviTargetPos);
_stateMachine.AddTransition(moveToSupply, alignToSupply, ReachedSupplyXZ());
_stateMachine.AddTransition(navigateToSupplyWait, moveToSupplyWait, ApproxNaviTargetPos);
_stateMachine.AddTransition(moveToSupplyWait, navigateToSupply, ReachedNaviPosSupplyIsCurrent());
_stateMachine.AddTransition(moveToSupplyWait, alignToSupplyWait, ReachedNaviPosSupplyIsWait());
_stateMachine.AddTransition(alignToSupplyWait, navigateToSupply, ReachedNaviRotSupplyIsCurrent());
_stateMachine.AddTransition(alignToSupplyWait, descendToSupplyWait, ReachedNaviRotSupplyIsWait());
_stateMachine.AddTransition(alignToSupply, descendToSupply, ReachedSupplyRot());
_stateMachine.AddTransition(descendToSupplyWait, waitAtSupply, ReachedSupplyWait());
_stateMachine.AddTransition(waitAtSupply, navigateToSupply, WaitForSupplyFinished());
_stateMachine.AddTransition(descendToSupply, resupply, ReachedSupplyPosIsCurrent());
_stateMachine.AddTransition(resupply, ascendFromSupply, () => true);
_stateMachine.AddTransition(ascendFromSupply, navigateToTarget, ReachedTransHeight());
_stateMachine.AddTransition(navigateToTarget, moveToTarget, ApproxNaviTargetPos);
_stateMachine.AddTransition(moveToTarget, alignToTarget, ReachedTargetXZ());
_stateMachine.AddTransition(alignToTarget, descendToTarget, ReachedTargetRot());
_stateMachine.AddTransition(descendToTarget, buildBlock, ReachedTarget());
_stateMachine.AddTransition(buildBlock, ascendFromTarget, () => true);
_stateMachine.AddTransition(ascendFromTarget, requestTarget, ReachedTransHeight());
_stateMachine.AddTransition(ascendFromTarget, navigateToDock, BatteryLow());
_stateMachine.SetState(idle);
Func <bool> IsRunning() => () => running && IsCharged();
Func <bool> AssignedTarget() => () => target != null;
Func <bool> WaitForTarget() => () =>
target == null && Vector3.Distance(transform.position, dock.position) > PosTolerance;
Func <bool> ReachedSupplyXZ() => () => target != null && supply != null &&
ReachedNaviTargetPos();
Func <bool> ReachedSupplyWait() => () => target != null && supply != null &&
ReachedNaviTargetPos();
Func <bool> WaitForSupplyFinished() => () => target != null && supply != null &&
supply.IsDroneCurrent(this);
Func <bool> ReachedTargetXZ() => () => target != null && ReachedNaviTargetPos();
Func <bool> ReachedTargetRot() => () => target != null && ReachedNaviTargetRot();
Func <bool> ReachedDockXZ() => () => target == null && ReachedNaviTargetPos();
Func <bool> ReachedTarget() => () => target != null && ReachedNaviTargetPos();
Func <bool> ReachedSupplyPosIsCurrent() => () =>
supply != null && ReachedNaviTargetPos() && supply.IsDroneCurrent(this) &&
Vector3.Distance(transform.position, supply.GetDroneAssignedTransform(this).position) <= PosTolerance;
Func <bool> ReachedSupplyRot() => () => supply != null && ReachedNaviTargetRot();
Func <bool> ReachedTransHeight() => () => Mathf.Abs(transform.position.y - TransHeight) < PosTolerance;
Func <bool> BatteryLow() => () => ReachedTransHeight()() && _battery <= BatteryThreshold;
Func <bool> ReachedNaviPosSupplyIsCurrent() => () => supply.IsDroneCurrent(this) && ReachedNaviTargetPos();
Func <bool> ReachedNaviPosSupplyIsWait() => () => supply.IsDroneWaiting(this) && ReachedNaviTargetPos();
Func <bool> ReachedNaviRotSupplyIsCurrent() => () => supply.IsDroneCurrent(this) && ReachedNaviTargetRot();
Func <bool> ReachedNaviRotSupplyIsWait() => () => supply.IsDroneWaiting(this) && ReachedNaviTargetRot();
bool ApproxNaviTargetPos() => Vector3.Distance(transform.position, _naviTarget.position) <= NavTolerance;
bool ReachedNaviTargetPos() => Vector3.Distance(transform.position, _naviTarget.position) <= PosTolerance;
bool ReachedNaviTargetRot() =>
Mathf.Abs(transform.eulerAngles.y - _naviTarget.rotation.eulerAngles.y) <= RotTolerance;
}
