ThumbState_t thumbSt;

const bool DEBUG = true;  // set to true to debug the raw values

int xPin = A2;
int yPin = A3;
int xBPin = A0;
int yBPin = A1;

int xZero, yZero;
int xValue, yValue;
int xBZero, yBZero;
int xBValue, yBValue;

int deadzone = 40;  // smaller values will be set to 0
int deadzoneB = 40;  // smaller values will be set to 0

void setup(){
 	pinMode(xPin, INPUT);
 	pinMode(yPin, INPUT);
 	pinMode(xBPin, INPUT);
 	pinMode(yBPin, INPUT);

 	if(DEBUG) {
 		Serial.begin(9600);
 	}

	// calculate neutral position
	xZero = analogRead(xPin);
	yZero = analogRead(yPin);
	xBZero = analogRead(xBPin);
	yBZero = analogRead(yBPin);


	thumbSt.xAxis = 0;
	thumbSt.yAxis = 0;
	thumbSt.xBAxis = 0;
	thumbSt.yBAxis = 0;

}

void loop(){
	xValue = analogRead(xPin) - xZero;
	yValue = analogRead(yPin) - yZero;
	xBValue = analogRead(xBPin) - xBZero;
	yBValue = analogRead(yBPin) - yBZero;


	if(abs(xValue) < deadzone) {
		xValue = 0;
	}
	if(abs(yValue) < deadzone) {
		yValue = 0;
	}
	if(abs(xBValue) < deadzoneB) {
		xBValue = 0;
	}
	if(abs(yBValue) < deadzoneB) {
		yBValue = 0;
	}





	thumbSt.xAxis = map(xValue, 370, -370, -32768, 32768);  // here the axis is inverted
	thumbSt.yAxis = map(yValue, -370, 370, -32768, 32768);
	thumbSt.xBAxis = map(xBValue, 300, -360, -32768, 32768);  // here the axis is inverted
	thumbSt.yBAxis = map(yBValue, -320, 320, -32768, 32768);




 	if(DEBUG) {
 		Serial.print("X: ");
		Serial.println(xValue);
 		Serial.print("Y: ");
		Serial.println(yValue);
 		Serial.print("XB: ");
		Serial.println(xBValue);
 		Serial.print("YB: ");
		Serial.println(yBValue);

	}

	// Send to USB
	Thumbstick.setState(&thumbSt);
}