//#define SCAN_MODE // If set, scan & output channel values, rather than driving the robot.

/////////////////////////////////////////////////////////////////////////
// Configuration
//////////////////////////////////////////////////

#define MAX_POWER_PERCENT 90 // The maximum percentage on time, used to decrease average voltage to the motor.
#define OUTPUT_RANGE 255 // The maximum value for PWM output
///////////////////////

#define SCAN_DELAY 100 // Delay between updates in ms
#define AVG_PERIODS 10 // Number of periods to average the speed over (reduces random variances)

/////////////////////////////////////////////////////////////////////////
// Channel pins
/////////////////////////////////////////////////////////////////////////

#define LOW_CHANNEL 1
#define NUM_CHANNELS 6

// Channel pins. Using numbers 1-6 for clarity, so using a bad value in index 0.
const byte channels[7] = {255, 8,9,10,11,12,13};

/////////////////////////////////////////////////////////////////////////
// Channel values
/////////////////////////////////////////////////////////////////////////

// Right horizontal (steering)
#define STEER_CHANNEL 1
#define STEER_LEFT 1300
#define STEER_RIGHT 1690
#define STEER_NONE 1500
#define STEER_DEAD_ZONE 30 // 10% of the range is dead zone

// Right vertical (throttle)
#define THROT_CHANNEL 2
#define THROT_MAX 1940
#define THROT_MIN 1160
#define THROT_NONE 1510// Halfway range
#define THROT_DEAD_ZONE 30 // approx 10% of the range is dead
#define THROT_OFF 0 // Anything below this means the throttle switch (upper right) is off

#define CH_4_MIN 0
#define CH_4_MAX 0


// Right knob
#define LIGHT_CHANNEL 5
#define LIGHT_MIN 990
#define LIGHT_MAX 1990

// Left knob
#define SPEED_CHANNEL 6
#define SPEED_MIN 1000
#define SPEED_MAX 2000

/////////////////////////////////////////////////////////////////////////
// Output pins
/////////////////////////////////////////////////////////////////////////

#define LEFT_MOTOR_PIN 5
#define LEFT_MOTOR_DIRECT_PIN 4
#define RIGHT_MOTOR_PIN 6
#define RIGHT_MOTOR_DIRECT_PIN 7

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

void setup() {
	Serial.begin(9600);

	pinMode(LEFT_MOTOR_PIN, OUTPUT);
	pinMode(RIGHT_MOTOR_PIN, OUTPUT);
	pinMode(LEFT_MOTOR_DIRECT_PIN, OUTPUT);
	pinMode(RIGHT_MOTOR_DIRECT_PIN, OUTPUT);

	for(byte i = LOW_CHANNEL; i <= NUM_CHANNELS; i++) {
		pinMode(channels[i], INPUT);
	}

	delay(2000);
}

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;
}