OrderedInput::OrderedInput(CSteppedInputLookahead & _input, unsigned _originalIndex, bool hasDistance) : originalIndex(_originalIndex)
{
input.set(&_input);
matched = false;
skipCost = 1; // This should be enhanced...
numSamples = 0;
nextDistanceIndex = 0;
distanceCalculator = NULL;
stepFlags = input->getStepFlags();
priority = input->getPriority();
if (hasDistance)
distanceCalculator = _input.queryDistance();
optimizeOrder = !hasPriority() && (distanceCalculator && input->canSmartStep());
// make it low initially to ensure that all inputs are sought to get some kind of idea of how good they are
medianDistance.set(0,0);
}
void JacobsLadder::addMovement(MovementType type, int velocity, byte angle, LadderCallback onStart, LadderCallback onEnd) {
if(queue.count() > 4) {
return;
}
if (!hasPriority(type)) {
return;
}
byte startingAngle = getFinalDestinationAngle();
byte destinationAngle = 0;
byte buzzAngle = 0;
int updateDelay = 0;
bool needsResetting = false;
bool shouldSendOnStart = true;
bool shouldSendOnEnd = true;
switch(type) {
case Cascade:
destinationAngle = 0;
if (startingAngle < 90) {
destinationAngle = 180;
}
updateDelay = calculateUpdateDelay(velocity, destinationAngle, startingAngle);
break;
case Buzz:
addMovement(Reset, velocity, 0, NULL, NULL);
buzzAngle = 30;
if (angle != 0) {
buzzAngle = angle;
}
destinationAngle = buzzAngle;
if (startingAngle > 90) {
destinationAngle = 180 - buzzAngle;
}
updateDelay = calculateUpdateDelay(velocity, destinationAngle, startingAngle);
needsResetting = true;
shouldSendOnEnd = false;
break;
case Reset:
if (startingAngle != 0 || startingAngle != 180) {
destinationAngle = 0;
if (startingAngle > 90) {
destinationAngle = 180;
}
updateDelay = calculateUpdateDelay(velocity, destinationAngle, startingAngle);
}
break;
case Timeout:
destinationAngle = startingAngle;
updateDelay = velocity;
break;
}
struct Movement movement;
movement.type = type;
movement.destinationAngle = destinationAngle;
movement.updateDelay = updateDelay;
movement.ratio = (float) movement.updateDelay / (int) (abs(movement.destinationAngle - startingAngle) != 0 ? abs(movement.destinationAngle - startingAngle) : 1);
if (shouldSendOnStart) {
movement.onStart = onStart;
} else {
movement.onStart = NULL;
}
if (shouldSendOnEnd) {
movement.onEnd = onEnd;
} else {
movement.onEnd = NULL;
}
queue.push(movement);
if (movement.type != Timeout) addMovement(Timeout, 10, 0, NULL, NULL);
if (needsResetting) {
addMovement(Reset, velocity, 0, NULL, onEnd);
addMovement(Timeout, 10, 0, NULL, NULL);
}
}
