#include <avr/pgmspace.h>

#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include "log_lookup.h"

// DFRobot I/O Expansion Shield V7 - http://www.dfrobot.com/wiki/index.php/IO_Expansion_Shield_for_Arduino_V7_SKU:DFR0265
// 2xPS3 joysticks connected like so
// Joy 1 Horizontal - Left/right   - AD0
// Joy 1 Vertical   - Forward/back - AD1
// Joy 1 Button     - Unused       - D2
// Joy 2 Horizontal - Unused       - AD2
// Joy 2 Vertical   - Up/down      - AD3
// Joy 2 Button     - Unused       - D3

// LCD panel
LiquidCrystal_I2C lcd(0x27, 16, 2);  // I/O expander is at 0x27, LCD is 16x2

void setup() {
	int byte;

	Serial.begin(115200);

	// Set joystick button pins as inputs
	pinMode(7, INPUT);
	pinMode(8, INPUT);

	lcd.init();
	lcd.backlight();
	lcd.print("Hello world!");
}

void loop() {
	int joy1X, joy1Y, joy2X, joy2Y, but1, but2;
	int lint, rint, vint, ldir, rdir, vdir, ethrot;
	float k, l, r, v, x, y;
	char lcdbuf[16 + 1]; // Buffer 1 line

	k = 1.5;

	joy1X = analogRead(A0);
	joy1Y = analogRead(A1);
	but1 = !digitalRead(7); // Buttons are active low
	joy2X = analogRead(A2);
	joy2Y = analogRead(A3);
	but2 = !digitalRead(8);

#if 0
	Serial.print("Joy 1 X: "); Serial.print(joy1X); Serial.print("  ");
	Serial.print("Y: "); Serial.print(joy1Y); Serial.print("   Button:  ");
	Serial.println(but1);
	Serial.print("Joy 2 X: "); Serial.print(joy2X); Serial.print("  ");
	Serial.print("Y: "); Serial.print(joy2Y); Serial.print("   Button:  ");
	Serial.println(but2);
#endif
	// Create deadband in the centre because they don't sit at 512
	if (joy1X > 500 && joy1X < 510) // Sits at 505
		joy1X = 512;
	if (joy1Y > 516 && joy1Y < 526) // Sits at 521
		joy1Y = 512;
	if (joy2X > 500 && joy2X < 510) // Sits at 505
		joy2X = 512;
	if (joy2Y > 520 && joy2Y < 530) // Sits at 525
		joy2Y = 512;

	// Mix 'joystick' input to left/right thrust (elevon mixing)
	// http://eastbay-rc.blogspot.com.au/2011/05/elevon-v-tail-mixing-calculations.html
	x = (joy1X - 512.0) / 512.0; // More positive = right
	y = (joy1Y - 512.0) / 512.0; // More positive = forward, convert to -1..1

	l =  1 * (x / 2 + y / 2);
        r = -1 * (x / 2 - y / 2);

	v = ((float)(joy2Y - 512) / 512.0);

	// Apply some gain
	l *= 2.2;
	r *= 2.2;

	// Limit
	if (l > 1)
		l = 1;
	if (l < -1)
		l = -1;
	if (r > 1)
		r = 1;
	if (r < -1)
		r = -1;
	if (v < -1)
		v = -1;
	if (v > 1)
		v = 1;

	// Map values to -255..255
	lint = l * 255;
	rint = r * 255;
	vint = v * 255;

	// Apply log transfer function
	ldir = lint < 0 ? 0 : 1; // 0 = reverse, 1 = forward
	lint = pgm_read_byte_near(log_lookup + abs(lint));
	rdir = rint < 0 ? 0 : 1;
	rint = pgm_read_byte_near(log_lookup + abs(rint));
	vdir = vint < 0 ? 0 : 1;
	vint = pgm_read_byte_near(log_lookup + abs(vint));

	Serial.print("L:");
	Serial.print(lint * (ldir == 0 ? -1 : 1));
	Serial.print(" R:");
	Serial.print(rint * (rdir == 0 ? -1 : 1));
	Serial.print(" V:");
	Serial.print(vint * (vdir == 0 ? -1 : 1));
	Serial.println();

	snprintf(lcdbuf, sizeof(lcdbuf) - 1, "L%c%3dR%c%3dV%c%3d", ldir == 0 ? '-' : '+', lint,
	    rdir == 0 ? '-' : '+', rint, vdir == 0 ? '-' : '+', vint);
	lcd.setCursor(0, 0);
	lcd.print(lcdbuf);
	snprintf(lcdbuf, sizeof(lcdbuf) - 1, "  B1: %d  B2: %d   ", but1, but2);
	lcd.setCursor(0, 1);
	lcd.print(lcdbuf);
	delay(200);
}

/*
 * Local variables:
 * mode: c++
 * End:
 */