char outputStr[100];
int16_t leftAverages[AVG_PERIODS];
int16_t rightAverages[AVG_PERIODS];

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

const int steerDeadHigh = STEER_NONE + STEER_DEAD_ZONE,
	steerDeadLow = STEER_NONE - STEER_DEAD_ZONE;


void loop() {
	const long startTime = millis();
	deadmanSwitch();
	
#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(),
		steer = getSteering();

	driveWheel(throttle, steer, LEFT),
	driveWheel(throttle, steer, RIGHT);

	Serial.println();
#endif

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


/**
 * Return the throttle value between -THROT_RANGE (reverse) and +THROT_RANGE (forward)
 */
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;
	}

	// Map the value to a value from 0 to THROT_RANGE range (or -THROT_RANGE to 0)
	int throttle = val < THROT_NONE
		? map(val, THROT_MIN, THROT_NONE, -THROT_RANGE, 0)
		: map(val, THROT_NONE, THROT_MAX, 0, THROT_RANGE);
	
	// Constrain the value in case the receiver outputs a value
	// outside the expected range.
	throttle = constrain(throttle, -THROT_RANGE, THROT_RANGE);

	debugln("Throttle: ", throttle);

	return throttle;
}


/**
 * Get the value for steering, between -STEER_RANGE (left) and +STEER_RANGE (right).
 */
int getSteering() {
	int val = channelVals[STEER_CHANNEL];

	debug("Steer channel: ");
	debug(val);
	debug("\t");

	if(val < steerDeadHigh && val > throtDeadLow) {
		debugln("Steering in dead zone");
		return 0;
	}

	// Map the value to a value from 0 to STEER_RANGE range (or -STEER_RANGE to 0)
	int steer = val < STEER_NONE
		? map(val, STEER_LEFT, STEER_NONE, -STEER_RANGE, 0)
		: map(val, STEER_NONE, STEER_RIGHT, 0, STEER_RANGE);

	// Constrain the value in case the receiver outputs a value
	// outside the expected range.
	steer = constrain(steer, -STEER_RANGE, STEER_RANGE);

	debugln("Steer: ", steer);

	return steer;
}


/**
 * Restrict the motor power based on the speed control dial.
 * Takes int from 0-255
 * Returns int 0-255
 */
int restrictThrottlePower(long 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);

	throttle = map(throttle, -THROT_RANGE, THROT_RANGE, -OUTPUT_RANGE, OUTPUT_RANGE);
	debugln("Range adjusted throttle:\t", throttle);
	
	return throttle;
}


/**
 * Get the average throttle for the last AVG_OVER_MILLIS ms
 *
 * Input: value from 0-255
 * Return: value from 0-255
 */
int getAverage(const int throttle, int16_t* averages) {
	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;
}


/**
 * Using the current throttle and steering values, determine the needed output
 * value for a given wheel (LEFT or RIGHT), then drive that motor.
 */
int driveWheel(int throttle, const int steer, const byte wheel) {
	// Negative steer = turn left = left backwards + right forwards
	// Positive steer = turn right = left forwards + right backwards
	int16_t* averages;
	debugln();
			
	switch(wheel) {
		case LEFT:
			debugln(F("=== LEFT ==="));
			Serial.print(F("Left:"));
			throttle += steer;
			averages = leftAverages;
			break;
		case RIGHT:
			debugln();
			debugln(F("=== RIGHT ==="));
			Serial.print(F("Right:"));
			throttle -= steer;
			averages = rightAverages;
			break;
	}

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

	// Make sure steering didn't throw it out of range
	throttle = constrain(throttle, -THROT_RANGE, THROT_RANGE);
	debugln("Constrained throttle:\t", throttle);

	// Restrict it to the max power dial
	throttle = restrictThrottlePower(throttle);

	// Average the power so it doesn't overwhelm the wheel
	throttle = getAverage(throttle, averages);

	debug("Wheel power: ");
	Serial.print(throttle);
	Serial.print(F("\t"));

	digitalWrite(
			wheel == LEFT ? LEFT_MOTOR_DIRECT_PIN : RIGHT_MOTOR_DIRECT_PIN,
			throttle > 0 ? HIGH : LOW
		);
	analogWrite(
			wheel == LEFT ? LEFT_MOTOR_PIN : RIGHT_MOTOR_PIN,
			throttle > 0 ? throttle : -throttle);
}