public override void Fire()
{
Init(); //be sure to leave this here
//get the external references
ModuleUKSN UKS = (ModuleUKSN)FindModuleByType(typeof(ModuleUKSN));
if (UKS == null)
{
return;
}
Module2DModel nmModel = (Module2DModel)FindModuleByType(typeof(Module2DModel));
if (nmModel == null)
{
return;
}
ModuleBehavior nmBehavior = (ModuleBehavior)FindModuleByType(typeof(ModuleBehavior));
if (nmBehavior == null)
{
return;
}
ModuleEvent mEvent = (ModuleEvent)FindModuleByType(typeof(ModuleEvent));
if (mEvent == null)
{
return;
}
//check on the various input neurons...
//check for a goal selection
foreach (Neuron n in na.Neurons())
{
if (n.Fired() && n.Label.IndexOf("c") == 0 && n.Model == Neuron.modelType.Std)
{
Thing newTarget = UKS.Labeled(n.Label);
if (newTarget == currentTarget)
{
currentTarget = null;
}
else
{
currentTarget = newTarget;
}
currentTargetReached = null;
break;
}
}
//check for operating mode
auto = (GetNeuronValue(null, "Auto") == 1) ? true : false;
//don't do anything while a behavior is in progress
if (GetNeuronValue("ModuleBehavior", "Done") == 0)
{
return;
}
//is there an existing landmark? Do I recognize I'm near a spot I was before?
//this only calculates "R" so it is rotation-independent
Thing best = null;
float bestDist = 1000;
List <Thing> near = nmModel.NearbySegments(3);
FindBestLandmarkMatch(UKS, ref best, ref bestDist, near);
nmModel.FireVisibleObjects(); //not really needed
if (bestDist < .2f) //are we near a landmark we've been at before?
{
SetNeuronValue(null, "Found", 1);
//yse, we have returned to a landmark we've been at before
currentLandmark = best;
currentEvent = currentLandmark.ReferencedBy[0].T;
//we need to reorient ourselves to face the same way as we did before (set a flag)
if (lastLandmark != currentLandmark)
{
lastLandmark = currentLandmark;
orienting = true;
}
}
else
{
//we're not near an existing landmark
currentLandmark = null;
currentEvent = null;
lastLandmark = null;
}
//this is on arrival back at a landmark
if (orienting)
{
Angle totalAngleError = GetTotalAngleError(near);
if (Abs(totalAngleError) > PI / 2 - .1f)
{
//turn in large increments until the angular error gets small
Angle angle = (float)PI / 2;
nmBehavior.TurnTo(angle);
return;
}
//this corrects for roundoff errors
CorrectModelPosition(nmModel, nmBehavior, best, near);
orienting = false;
return;
}
//we are in going-to-goal mode
if (currentTarget != null) //goingToGoal)
{
if (currentEvent != null) //currentEvent means we're at a decision point...check for the decision and execute it
{
Thing action = GoToGoal(currentTarget);
if (action != null)
{
float angle = GetAngleFromAction(action);
if (angle != 0)
{
nmBehavior.TurnTo(angle);
}
nmBehavior.MoveTo(1);
currentEvent = null;
return;
}
}
}
//we are in exploration mode
//We're not at a known landmark
//decide which way to turn at an obstacle
//If I am up to an obstacle...is there a decision or can I only go one way
float distAhead = nmModel.GetDistanceAtDirection(0);
float distLeft = nmModel.GetDistanceAtDirection((float)PI / 2);
float distRight = nmModel.GetDistanceAtDirection((float)-PI / 2);
bool canGoAhead = distAhead > 1;
bool canGoLeft = distLeft > 1;
bool canGoRight = distRight > 1;
int options = (canGoAhead ? 1 : 0) + (canGoRight ? 1 : 0) + (canGoLeft ? 1 : 0);
//First determine if there is a decision to be made or if there is only one option
if (options == 1 && auto)
{
//we have no choice but to follow the path
if (canGoAhead)
{
nmBehavior.MoveTo(1);
return;
}
if (canGoLeft)
{
nmBehavior.TurnTo((float)-PI / 2);
nmBehavior.MoveTo(1);
return;
}
if (canGoRight)
{
nmBehavior.TurnTo((float)PI / 2);
nmBehavior.MoveTo(1);
return;
}
}
else if (options == 0 && auto)
{
//we're trapped...note the color ahead and turn around
Thing thingAhead = nmModel.GetNearestThing();
currentTargetReached = thingAhead.References[2].T;
if (mostRecentDecisionPoint != null)
{
mEvent.AddOutcomePair(mostRecentDecisionPoint, mostRecentAction, currentTargetReached);
mostRecentAction = null;
mostRecentDecisionPoint = null;
}
Neuron n1 = null;
if (currentTargetReached != null)
{
n1 = na.GetNeuronAt(currentTargetReached.Label);
}
//color a new goal neuron
if (n1 == null && currentTargetReached != null)
{
n1 = AddLabel(currentTargetReached.Label + " ");
n1.Model = Neuron.modelType.Color;
n1.SetValueInt((int)currentTargetReached.V);
na.GetNeuronLocation(n1, out int X, out int Y);
Neuron n2 = na.GetNeuronAt(X + 1, Y);
if (n2 != null)
{
n2.Label = currentTargetReached.Label;
}
}
if (currentTargetReached == currentTarget)
{
//if we're looking for a goal, we've reached it, so stop
//currentTarget = null;
currentEvent = null;
}
else
{
//make a U-Turn and start backtracking
nmBehavior.TurnTo((float)-PI);
}
}
else if (auto)
{
//we have a choice...
//if the current landmark is null, create a new landmark & Event
if (currentLandmark == null)
{
//Create new Landmark...it clones the points so they are not modified by the model module
Thing newLandmark = mEvent.CreateLandmark(near);
currentEvent = mEvent.CreateEvent(newLandmark);
currentLandmark = newLandmark;
if (mostRecentDecisionPoint != null)
{
mEvent.AddOutcomePair(mostRecentDecisionPoint, mostRecentAction, currentEvent);
}
}
else
{
if (mostRecentDecisionPoint != null && mostRecentDecisionPoint.Children.Find(t1 => t1.References[0].T == mostRecentAction) == null)
{
mEvent.AddOutcomePair(mostRecentDecisionPoint, mostRecentAction, currentEvent);
}
}
//TODO improve the method of finding something not tried before
//Decide which untried path to take
//priorities...1)continue ahead 2)left 3)right or randomized (depending on comment below)
Thing newAction = UKS.Labeled("NoAction");
List <Thing> possibleActions = new List <Thing>();
if (currentEvent.Children.Find(t => t.References[0].T.Label == "GoS") == null && canGoAhead)
{
possibleActions.Add(UKS.Labeled("GoS"));
}
if (currentEvent.Children.Find(t => t.References[0].T.Label == "LTurnS") == null && canGoLeft)
{
possibleActions.Add(UKS.Labeled("LTurnS"));
}
if (currentEvent.Children.Find(t => t.References[0].T.Label == "RTurnS") == null && canGoRight)
{
possibleActions.Add(UKS.Labeled("RTurnS"));
}
if (possibleActions.Count == 0 && currentEvent.Children.Find(t => t.References[0].T.Label == "UTurnS") == null)
{
newAction = UKS.Labeled("UTurnS");
}
else if (possibleActions.Count == 1)
{
newAction = possibleActions[0];
}
else if (possibleActions.Count > 0)
{
//for debugging, eliminate the ransomization by alternately commenting the 2 stmts below
// newAction = possibleActions[0];
newAction = possibleActions[rand.Next(possibleActions.Count)];
}
if (newAction.Label != "NoAction")
{
mostRecentAction = newAction;
mostRecentDecisionPoint = currentEvent;
Angle angle = GetAngleFromAction(newAction);
nmBehavior.TurnTo(angle);
nmBehavior.MoveTo(1);
}
else
{
//TODO: all actions at the current Event have been tried, is there another Event which hasn't been exhausted?
}
lastLandmark = currentLandmark;
return;
}
}