char outputStr[100];
int16_t averages[AVG_PERIODS];

const int throtDeadHigh = THROT_NONE + THROT_DEAD_ZONE,
	throtDeadLow = THROT_NONE - THROT_DEAD_ZONE;

void loop() {
	const long startTime = millis();
	
#ifdef SCAN_MODE
	for(byte ch = LOW_CHANNEL; ch <= NUM_CHANNELS; ch++) {
		int val = channelVals[ch];
		sprintf(outputStr, "Channel-%d:%d\t", ch, val);
		Serial.print(outputStr);
	}
	Serial.println();
#else
	debugln();
	debugln("===================");

	int throttle = getThrottle();
	throttle = restrictThrottlePower(throttle);
	throttle = getAverage(throttle);

	sprintf(outputStr, "Throttle:%d", throttle);
	Serial.println(outputStr);

	digitalWrite(LEFT_MOTOR_DIRECT_PIN, throttle > 0 ? HIGH : LOW);
	analogWrite(LEFT_MOTOR_PIN, throttle > 0 ? throttle : -throttle);
#endif

	const long now = millis();
	delay(SCAN_DELAY + (startTime - now));
}

int getThrottle() {
	int val = channelVals[THROT_CHANNEL];

	debug("Channel value: ");
	debug(val);
	debug("\t");
	if(val <= THROT_OFF) {
		debugln("Throttle is off");
		return 0;
	} else if(val < throtDeadHigh && val > throtDeadLow) {
		debugln("Throttle in dead zone");
		return 0;
	}
	

	int throttle = val < THROT_NONE
		? map(val, THROT_MIN, THROT_NONE, -OUTPUT_RANGE, 0)
		: map(val, THROT_NONE, THROT_MAX, 0, OUTPUT_RANGE);
	
	throttle = constrain(throttle, -OUTPUT_RANGE, OUTPUT_RANGE);

	debugln("Throttle: ", throttle);

	return throttle;
}

int restrictThrottlePower(int throttle) {
	// Reduce the average voltage to something safe for the motor to consume.
	throttle = (throttle * MAX_POWER_PERCENT) / 100;

	debugln("Restricted throttle:\t", throttle);

	// Use the current value of the knob to reduce the speed.
	int speedKnob = channelVals[SPEED_CHANNEL];
	speedKnob = map(speedKnob, SPEED_MIN, SPEED_MAX, 0, 100);
	speedKnob = constrain(speedKnob, 0, 100);
	throttle = (throttle * speedKnob) / 100;

	debugln("Speed knob:\t", speedKnob);
	debugln("Speed adjusted throttle:\t", throttle);
	
	return throttle;
}

int getAverage(const int throttle) {
	long long total = throttle;
	for(byte i = 0; i < AVG_PERIODS-1; i++) {
		total += averages[i];
		averages[i] = averages[i+1];
	}
	averages[AVG_PERIODS-1] = throttle;
	total /= AVG_PERIODS;

	debugln("Average adjusted throttle:\t", total);

	return total;
}