#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!");
}

// Based on http://playground.arduino.cc/Main/Fscale
float
fscale(float originalMin, float originalMax, float newBegin,
    float newEnd, float inputValue, float curve) {
	float OriginalRange = 0;
	float NewRange = 0;
	float zeroRefCurVal = 0;
	float normalizedCurVal = 0;
	float rangedValue = 0;
	boolean invFlag = 0;

	// condition curve parameter
	// limit range
	if (curve > 10) curve = 10;
	if (curve < -10) curve = -10;

	curve = (curve * -.1) ; // - invert and scale - this seems more intuitive - postive numbers give more weight to high end on output
	curve = pow(10, curve); // convert linear scale into lograthimic exponent for other pow function

	/*
	  Serial.println(curve * 100, DEC);   // multply by 100 to preserve resolution
	  Serial.println();
	*/

	// Check for out of range inputValues
	if (inputValue < originalMin) {
		inputValue = originalMin;
	}
	if (inputValue > originalMax) {
		inputValue = originalMax;
	}

	// Zero Reference the values
	OriginalRange = originalMax - originalMin;

	if (newEnd > newBegin) {
		NewRange = newEnd - newBegin;
	} else {
		NewRange = newBegin - newEnd;
		invFlag = 1;
	}

	zeroRefCurVal = inputValue - originalMin;
	normalizedCurVal  =  zeroRefCurVal / OriginalRange;   // normalize to 0 - 1 float

	/*
	  Serial.print(OriginalRange, DEC);
	  Serial.print("   ");
	  Serial.print(NewRange, DEC);
	  Serial.print("   ");
	  Serial.println(zeroRefCurVal, DEC);
	  Serial.println();
	*/

	// Check for originalMin > originalMax  - the math for all other cases i.e. negative numbers seems to work out fine
	if (originalMin > originalMax) {
		return 0;
	}

	if (invFlag == 0) {
		rangedValue = (pow(normalizedCurVal, curve) * NewRange) + newBegin;

	} else {
		// invert the ranges
		rangedValue = newBegin - (pow(normalizedCurVal, curve) * NewRange);
	}

	return rangedValue;
}

void loop() {
	int joy1X, joy1Y, joy2X, joy2Y, but1, but2;
	int lint, rint, vint, ldir, rdir, vdir;
	float k, l, r, v, x, y, tmp;
	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;

	// Convert each axis to -1..1
	x = ((float)joy1X - 512.0) / 512.0;
	y = ((float)joy1Y - 512.0) / 512.0;
	v = ((float)joy2Y - 512.0) / 512.0;

	// Mix 'joystick' input to left/right thrust (elevon mixing)
	// http://eastbay-rc.blogspot.com.au/2011/05/elevon-v-tail-mixing-calculations.html
	l =  1 * (x / 2 + y / 2);
        r = -1 * (x / 2 - y / 2);

	// Apply some gain so full forward is a max speed
	l *= 2.0;
	r *= 2.0;

	// Apply transfer function and limit
	tmp = fscale(0, 1, 0, 1, abs(l), -1.5);
#if 0
	Serial.print("L: "); Serial.print(l); Serial.print(" Tmp: "); Serial.print(tmp); Serial.println();
#endif
	l = tmp * (l < 0 ? -1 : 1);
	tmp = fscale(0, 1, 0, 1, abs(r), -1.5);
	r = tmp * (r < 0 ? -1 : 1);
	tmp = fscale(0, 1, 0, 1, abs(v), -1.5);
	v = tmp * (v < 0 ? -1 : 1);

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

	Serial.print("L:");
	Serial.print(lint);
	Serial.print(" R:");
	Serial.print(rint);
	Serial.print(" V:");
	Serial.print(vint);
	Serial.println();

	snprintf(lcdbuf, sizeof(lcdbuf) - 1, "L%3dR%3dV%3d", lint, rint, 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:
 */