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


void loop() {
#ifdef SCAN_MODE
	for(byte i = LOW_CHANNEL; i <= NUM_CHANNELS; i++) {
		readChannel(i);
	}
#else
	Serial.println("============");
	int throttle = getThrottle();
	throttle = restrictThrottlePower(throttle);
	throttle = getAverage(throttle);

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

	Serial.println();
	delay(SCAN_DELAY);
}

#define TIMEOUT_MICROS 30000

uint16_t readChannel(byte channel) {
	int val = pulseIn(channels[channel], HIGH, TIMEOUT_MICROS);

#ifdef SCAN_MODE
	if(val > 100) val -= 1000;
	if(channel > 1) Serial.print(" ");
	Serial.print(val);
#else
	sprintf(outputStr, "Channel %d:\t%d", channel, val);
	Serial.println(outputStr);
#endif
	return val;
}

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

int getThrottle() {
	int val = readChannel(THROT_CHANNEL);
	Serial.print("Channel value: ");
	Serial.print(val);
	Serial.print("\t");
	if(val <= THROT_OFF) {
		Serial.println("Throttle is off");
		return 0;
	} else if(val < throtDeadHigh && val > throtDeadLow) {
		Serial.println("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);
	Serial.print("Throttle: ");
	Serial.println(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;
	Serial.print("Restricted throttle:\t");
	Serial.println(throttle);

	// Use the current value of the knob to reduce the speed.
	int speedKnob = readChannel(SPEED_CHANNEL);
	speedKnob = map(speedKnob, SPEED_MIN, SPEED_MAX, 0, 100);
	speedKnob = constrain(speedKnob, 0, 100);
	throttle = (throttle * speedKnob) / 100;
	Serial.print("Speed knob:\t");
	Serial.println(speedKnob);
	Serial.print("Speed adjusted throttle:\t");
	Serial.println(throttle);
	
	return throttle;
}

int getAverage(const int throttle) {
	int 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;
	Serial.print("Average adjusted throttle:\t");
	Serial.println(total);
	return total;
}