#include <Wire.h>
#include <Servo.h>

#include <Max3421e.h>
#include <Usb.h>
#include <AndroidAccessory.h>

#define  LED3_RED       2
#define  LED3_GREEN     3
#define  LED3_BLUE      4

#define  LED2_RED       5
#define  LED2_GREEN     6
#define  LED2_BLUE      7

#define  LED1_RED       8
#define  LED1_GREEN     9
#define  LED1_BLUE      10

#define  SERVO1         11
#define  SERVO2         12
#define  SERVO3         13

#define  LIGHT_SENSOR   A4
#define  TEMP_SENSOR    A3

#define  BUTTON1        A0
#define  BUTTON2        A1
#define  BUTTON3        A2

#define JOY_X           A5
#define JOY_Y           A7


AndroidAccessory acc("Google, Inc.",
		     "DemoKit",
		     "DemoKit Arduino Board",
		     "1.0",
		     "http://www.android.com",
		     "0000000012345678");
Servo servos[3];

void setup();
void loop();

void init_buttons()
{
	pinMode(BUTTON1, INPUT);
	pinMode(BUTTON2, INPUT);
	pinMode(BUTTON3, INPUT);

	// enable the internal pullups
	digitalWrite(BUTTON1, HIGH);
	digitalWrite(BUTTON2, HIGH);
	digitalWrite(BUTTON3, HIGH);
}
void init_leds()
{
	digitalWrite(LED1_RED, 1);
	digitalWrite(LED1_GREEN, 1);
	digitalWrite(LED1_BLUE, 1);

	pinMode(LED1_RED, OUTPUT);
	pinMode(LED1_GREEN, OUTPUT);
	pinMode(LED1_BLUE, OUTPUT);

	digitalWrite(LED2_RED, 1);
	digitalWrite(LED2_GREEN, 1);
	digitalWrite(LED2_BLUE, 1);

	pinMode(LED2_RED, OUTPUT);
	pinMode(LED2_GREEN, OUTPUT);
	pinMode(LED2_BLUE, OUTPUT);

	digitalWrite(LED3_RED, 1);
	digitalWrite(LED3_GREEN, 1);
	digitalWrite(LED3_BLUE, 1);

	pinMode(LED3_RED, OUTPUT);
	pinMode(LED3_GREEN, OUTPUT);
	pinMode(LED3_BLUE, OUTPUT);
}

byte b1, b2, b3, c;
void setup()
{
	Serial.begin(115200);
	Serial.print("\r\nStart");

	init_leds();
	init_buttons();

	servos[0].attach(SERVO1);
	servos[0].write(90);
	servos[1].attach(SERVO2);
	servos[1].write(90);
	servos[2].attach(SERVO3);
	servos[2].write(90);


	b1 = digitalRead(BUTTON1);
	b2 = digitalRead(BUTTON2);
	b3 = digitalRead(BUTTON3);
	c = 0;

	acc.powerOn();
}

void loop()
{
	byte err;
	byte idle;
	static byte count = 0;
	byte msg[3];
	long touchcount;

	if (acc.isConnected()) {
		int len = acc.read(msg, sizeof(msg), 1);
		int i;
		byte b;
		uint16_t val;
		int x, y;
		char c0;
                int aval;

		if (len > 0) {
			// assumes only one command per packet
			if (msg[0] == 0x2) {
                                  //convert liner to  exponential value
                                float val = (float)msg[2] / 255;
                                val = (val * val) * 255;
                                if(val < 0) {
                                  aval = 0;
                                }else if (val > 255){
                                  aval = 255;
                                }else{
                                  aval = (int)val;
                                }
                                aval = 255 - (aval / 2);
                               

				if (msg[1] == 0x0)
					analogWrite(LED1_RED, aval);
				else if (msg[1] == 0x1)
					analogWrite(LED1_GREEN, aval);
				else if (msg[1] == 0x2)
					analogWrite(LED1_BLUE, aval);
				else if (msg[1] == 0x3)
					analogWrite(LED2_RED, aval);
				else if (msg[1] == 0x4)
					analogWrite(LED2_GREEN, aval);
				else if (msg[1] == 0x5)
					analogWrite(LED2_BLUE, aval);
				else if (msg[1] == 0x6)
					analogWrite(LED3_RED, aval);
				else if (msg[1] == 0x7)
					analogWrite(LED3_GREEN, aval);
				else if (msg[1] == 0x8)
					analogWrite(LED3_BLUE, aval);
				else if (msg[1] == 0x10)
					servos[0].write(map(msg[2], 0, 255, 0, 180));
				else if (msg[1] == 0x11)
					servos[1].write(map(msg[2], 0, 255, 0, 180));
				else if (msg[1] == 0x12)
					servos[2].write(map(msg[2], 0, 255, 0, 180));
			} else if (msg[0] == 0x3) {

			}
		}

		msg[0] = 0x1;

		b = digitalRead(BUTTON1);
		if (b != b1) {
			msg[1] = 0;
			msg[2] = b ? 0 : 1;
			acc.write(msg, 3);
			b1 = b;
		}

		b = digitalRead(BUTTON2);
		if (b != b2) {
			msg[1] = 1;
			msg[2] = b ? 0 : 1;
			acc.write(msg, 3);
			b2 = b;
		}

		b = digitalRead(BUTTON3);
		if (b != b3) {
			msg[1] = 2;
			msg[2] = b ? 0 : 1;
			acc.write(msg, 3);
			b3 = b;
		}

		switch (count++ % 0x10) {
		case 0:
			val = analogRead(TEMP_SENSOR);
			msg[0] = 0x4;
			msg[1] = val >> 8;
			msg[2] = val & 0xff;
			acc.write(msg, 3);
			break;

		case 0x4:
			val = analogRead(LIGHT_SENSOR);
			msg[0] = 0x5;
			msg[1] = val >> 8;
			msg[2] = val & 0xff;
			acc.write(msg, 3);
			break;

		case 0x8:
			read_joystick(&x, &y);
//                        x = 0; y = 0;
			msg[0] = 0x6;
			msg[1] = constrain(x, -128, 127);
			msg[2] = constrain(y, -128, 127);
			acc.write(msg, 3);
			break;

		case 0xc:
//			touchcount = touch_robot.capSense(5);
			touchcount = 0;

			c0 = touchcount > 750;

			if (c0 != c) {
				msg[0] = 0x1;
				msg[1] = 3;
				msg[2] = c0;
				acc.write(msg, 3);
				c = c0;
			}

			break;
		}
	} else {
		// reset outputs to default values on disconnect
		analogWrite(LED1_RED, 255);
		analogWrite(LED1_GREEN, 255);
		analogWrite(LED1_BLUE, 255);
		analogWrite(LED2_RED, 255);
		analogWrite(LED2_GREEN, 255);
		analogWrite(LED2_BLUE, 255);
		analogWrite(LED3_RED, 255);
		analogWrite(LED3_GREEN, 255);
		analogWrite(LED3_BLUE, 255);
		servos[0].write(90);
		servos[0].write(90);
		servos[0].write(90);
	}

	delay(10);
}

void read_joystick(int *x, int *y)
{
        uint16_t val;
        val = analogRead(JOY_X) / 4;
	*x =  val - 128;
        val = analogRead(JOY_Y) / 4;
	*y =  val - 128;
}