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

#define USB_ACCESSORY_VENDOR_ID 0x18D1
#define USB_ACCESSORY_PRODUCT_ID 0x2D00

#define USB_ACCESSORY_ADB_PRODUCT_ID 0x2D01
#define ACCESSORY_STRING_MANUFACTURER 0
#define ACCESSORY_STRING_MODEL 1
#define ACCESSORY_STRING_TYPE 2
#define ACCESSORY_STRING_VERSION 3

#define ACCESSORY_SEND_STRING 52
#define ACCESSORY_START 53


#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  TOUCH          14

#define  RELAY1         A0
#define  RELAY2         A1

#define  LIGHT_SENSOR   A2
#define  TEMP_SENSOR    A3

#define  BUTTON1        A6
#define  BUTTON2        A7
#define  BUTTON3        A8

#define  JOY_SWITCH     A9      // pulls line down when pressed
#define  JOY_nINT       A10     // active low interrupt input
#define  JOY_nRESET     A11     // active low reset output


MAX3421E Max;
USB Usb;
Servo servos[3];


void setup();
void loop();

uint8_t usbBuff[256];


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

	digitalWrite( BUTTON1, HIGH );  // enable the internal pullups
	digitalWrite( BUTTON2, HIGH );
	digitalWrite( BUTTON3, HIGH );
}


void init_relays()
{
	pinMode( RELAY1, OUTPUT );
	pinMode( RELAY2, OUTPUT );
}


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

void init_joystick( int threshold );

void setup()
{
	Serial.begin( 115200 );
	Serial.print("\r\nStart");

	init_leds();
	init_relays();
	init_buttons();
	init_joystick( 5 );      // initialize with thresholding enabled, dead zone of 5 units  


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

	Max.powerOn();
	delay( 200 );
}

bool isAndroidVendor(USB_DEVICE_DESCRIPTOR *desc)
{
	return desc->idVendor == 0x18d1 || desc->idVendor == 0x22B8;
}

bool isAccessoryDevice(USB_DEVICE_DESCRIPTOR *desc)
{
	return desc->idProduct == 0x2D00 || desc->idProduct == 0x2D01;
}

void sendString(byte addr, int index, char *str)
{
	Usb.ctrlReq(addr, 0, USB_SETUP_HOST_TO_DEVICE | USB_SETUP_TYPE_VENDOR | USB_SETUP_RECIPIENT_DEVICE,
		    ACCESSORY_SEND_STRING, 0, 0, index, strlen(str) + 1, str);

}

void switchDevice(byte addr)
{
	sendString(addr, ACCESSORY_STRING_MANUFACTURER, "Google, Inc.");
	sendString(addr, ACCESSORY_STRING_MODEL, "DemoKit");
	sendString(addr, ACCESSORY_STRING_TYPE, "Sample Program");
	sendString(addr, ACCESSORY_STRING_VERSION, "1.0");

	Usb.ctrlReq(addr, 0, USB_SETUP_HOST_TO_DEVICE | USB_SETUP_TYPE_VENDOR | USB_SETUP_RECIPIENT_DEVICE,
		    ACCESSORY_START, 0, 0, 0, 0, NULL);
}

bool findEndpoints(byte addr, EP_RECORD *inEp, EP_RECORD *outEp)
{
	int len;
	byte err;
	uint8_t *p;

	err = Usb.getConfDescr(addr, 0, 4, 0, (char *)usbBuff);
	if (err) {
		Serial.print("Can't get config descriptor length\n");
		return false;
	}

	len = usbBuff[2] | ((int)usbBuff[3] << 8);
	Serial.print("Config Desc Length: ");
	Serial.println(len, DEC);
	if (len > sizeof(usbBuff)) {
		Serial.print("config descriptor too large\n");
		/* might want to truncate here */
		return false;
	}

	err = Usb.getConfDescr(addr, 0, len, 0, (char *)usbBuff);
	if (err) {
		Serial.print("Can't get config descriptor\n");
		return false;
	}

	p = usbBuff;
	inEp->epAddr = 0;
	outEp->epAddr = 0;
	while (p < (usbBuff + len)){
		uint8_t descLen = p[0];
		uint8_t descType = p[1];
		USB_ENDPOINT_DESCRIPTOR *epDesc;
		EP_RECORD *ep;

		switch (descType) {
		case USB_DESCRIPTOR_CONFIGURATION:
			Serial.print("config desc\n");
			break;

		case USB_DESCRIPTOR_INTERFACE:
			Serial.print("interface desc\n");
			break;

		case USB_DESCRIPTOR_ENDPOINT:
			epDesc = (USB_ENDPOINT_DESCRIPTOR *)p;
			if (!inEp->epAddr && (epDesc->bEndpointAddress & 0x80))
				ep = inEp;
			else if (!outEp->epAddr)
				ep = outEp;
			else
				ep = NULL;

			if (ep) {
				ep->epAddr = epDesc->bEndpointAddress & 0x7f;
				ep->Attr = epDesc->bmAttributes;
				ep->MaxPktSize = epDesc->wMaxPacketSize;
				ep->sndToggle = bmSNDTOG0;
				ep->rcvToggle = bmRCVTOG0;
			}
			break;

		default:
			Serial.print("unkown desc type ");
			Serial.println( descType, HEX);
			break;
		}

		p += descLen;
	}

	return inEp->epAddr && outEp->epAddr;
}

EP_RECORD ep_record[ 8 ];  //endpoint record structure for the mouse


void doAndroid(void)
{
	byte err;
	byte idle;
	byte b1, b2, b3, c;
	EP_RECORD inEp, outEp;
	byte count = 0;

	if (findEndpoints(1, &inEp, &outEp)) {

		ep_record[inEp.epAddr] = inEp;
		if (outEp.epAddr != inEp.epAddr)
			ep_record[outEp.epAddr] = outEp;

		Serial.print("inEp: ");
		Serial.println(inEp.epAddr, HEX);
		Serial.print("outEp: ");
		Serial.println(outEp.epAddr, HEX);

		ep_record[0] = *(Usb.getDevTableEntry(0,0));
		Usb.setDevTableEntry(1, ep_record);

		err = Usb.setConf( 1, 0, 1 );
		if (err)
			Serial.print("Can't set config to 1\n");

		Usb.setUsbTaskState( USB_STATE_RUNNING );

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

		while(1) {
			int len = Usb.newInTransfer(1, inEp.epAddr, sizeof(usbBuff),
						    (char *)usbBuff);
			/* int i; */
			/* byte b; */
			/* byte msg[3]; */
			/* msg[0] = 0x1; */

			if (len > 0) {
				// XXX: assumes only one command per packet
				if (usbBuff[0] == 0x2) {
					if (usbBuff[1] == 0x0)
						analogWrite( LED1_RED, 255 - usbBuff[2]);
					else if (usbBuff[1] == 0x1)
						analogWrite( LED1_GREEN, 255 - usbBuff[2]);
					else if (usbBuff[1] == 0x2)
						analogWrite( LED1_BLUE, 255 - usbBuff[2]);
					else if (usbBuff[1] == 0x3)
						analogWrite( LED2_RED, 255 - usbBuff[2]);
					else if (usbBuff[1] == 0x4)
						analogWrite( LED2_GREEN, 255 - usbBuff[2]);
					else if (usbBuff[1] == 0x5)
						analogWrite( LED2_BLUE, 255 - usbBuff[2]);
					else if (usbBuff[1] == 0x6)
						analogWrite( LED3_RED, 255 - usbBuff[2]);
					else if (usbBuff[1] == 0x7)
						analogWrite( LED3_GREEN, 255 - usbBuff[2]);
					else if (usbBuff[1] == 0x8)
						analogWrite( LED3_BLUE, 255 - usbBuff[2]);
					else if (usbBuff[1] == 0x10)
						servos[0].write(map(usbBuff[2], 0, 255, 0, 180));
					else if (usbBuff[1] == 0x11)
						servos[1].write(map(usbBuff[2], 0, 255, 0, 180));
					else if (usbBuff[1] == 0x12)
						servos[2].write(map(usbBuff[2], 0, 255, 0, 180));
				} else if (usbBuff[0] == 0x3) {
					if (usbBuff[1] == 0x0)
						digitalWrite( RELAY1, usbBuff[2] ? HIGH : LOW );
					else if (usbBuff[1] == 0x1)
						digitalWrite( RELAY2, usbBuff[2] ? HIGH : LOW );

				}

//				for (i = 0; i < len; i++)
//				Serial.print('\n');
			}


		}

	}

}


void loop()
{
	USB_DEVICE_DESCRIPTOR *devDesc = (USB_DEVICE_DESCRIPTOR *) usbBuff;
	byte err;

	Max.Task();
	Usb.Task();
	if( Usb.getUsbTaskState() >= USB_STATE_CONFIGURING ) {
		Serial.print("\nDevice addressed... ");
		Serial.print("Requesting device descriptor.");

		err = Usb.getDevDescr(1, 0, 0x12, (char *) devDesc);
		if (err) {
			Serial.print("\nDevice descriptor cannot be retrieved. Program Halted\n");
			while(1);
		}

		if (isAndroidVendor(devDesc)) {
			Serial.print("found android device\n");

			if (isAccessoryDevice(devDesc)) {
				Serial.print("found android acessory device\n");
				doAndroid();
			} else {
				Serial.print("found possible device. swithcing to serial mode\n");
				switchDevice(1);
			}
		}

		while (Usb.getUsbTaskState() != USB_DETACHED_SUBSTATE_WAIT_FOR_DEVICE) {
			Max.Task();
			Usb.Task();


		}

		Serial.print("detached\n");

	}

}

// ==============================================================================
// Austria Microsystems i2c Joystick

/*
  If a threshold is provided, the dead zone will be programmed such that interrupts will not
  be generated unless the threshold is exceeded.

  Note that if you use that mode, you will have to use passage of time with no new interrupts
  to detect that the stick has been released and has returned to center.
  
  If you need to explicitly track return to center, pass 0 as the threshold.  "Center" will
  still bounce around a little 
*/


void init_joystick( int threshold )
{
  byte status = 0;
  
  pinMode( JOY_SWITCH, INPUT );
  digitalWrite( JOY_SWITCH, HIGH );    // enable the internal pullup
  
  pinMode( JOY_nINT, INPUT );
  digitalWrite( JOY_nINT, HIGH );      // enable the internal pullup

  pinMode( JOY_nRESET, OUTPUT );

  digitalWrite( JOY_nRESET, 1 );
  delay(1);
  digitalWrite( JOY_nRESET, 0 );
  delay(1);
  digitalWrite( JOY_nRESET, 1 );

  Wire.begin();
  
  do {
    status = read_joy_reg( 0x0f );        // XXX need timeout
  } while ((status & 0xf0) != 0xf0);
  
  write_joy_reg( 0x2e, 0x86 );            // invert magnet polarity setting, per datasheet

  calibrate_joystick( threshold );        // calibrate & set up dead zone area  
}


int offset_X, offset_Y;

void calibrate_joystick( int dz )
{
  char iii;
  int x_cal = 0;
  int y_cal = 0;

  write_joy_reg( 0x0f, 0x00 );          // Low Power Mode, 20ms auto wakeup
                                        // INTn output enabled
                                        // INTn active after each measurement
                                        // Normal (non-Reset) mode
  delay(1);

  read_joy_reg( 0x11 );                 // dummy read of Y_reg to reset interrupt

  for( iii = 0; iii != 16; iii++ ) {    // read coords 16 times & average 
    while( !joystick_interrupt() )      // poll for interrupt
      ;
    x_cal += read_joy_reg( 0x10 );      // X pos
    y_cal += read_joy_reg( 0x11 );      // Y pos
  }
  
  offset_X = -(x_cal>>4);               // divide by 16 to get average
  offset_Y = -(y_cal>>4);
  
  //sprintf(msgbuf, "offsets = %d, %d\n", offset_X, offset_Y);
  //Serial.print(msgbuf);
  
  write_joy_reg( 0x12,  dz - offset_X );  // Xp, LEFT threshold for INTn
  write_joy_reg( 0x13, -dz - offset_X );  // Xn, RIGHT threshold for INTn
  write_joy_reg( 0x14,  dz - offset_Y );  // Yp, UP threshold for INTn
  write_joy_reg( 0x15, -dz - offset_Y );  // Yn, DOWN threshold for INTn

  if ( dz )                             // dead zone threshold detect requested?
    write_joy_reg( 0x0f, 0x04 );          // Low Power Mode, 20ms auto wakeup
                                          // INTn output enabled
                                          // INTn active when movement exceeds dead zone
                                          // Normal (non-Reset) mode
}


void read_joystick( int *x, int *y )
{
  *x = read_joy_reg( 0x10 ) + offset_X;
  *y = read_joy_reg( 0x11 ) + offset_Y;  // reading Y clears the interrupt
}

char joystick_interrupt()
{
  return ( digitalRead( JOY_nINT ) == 0 ); 
}


#define  JOY_I2C_ADDR    0x40

char read_joy_reg( char reg_addr )
{
  char c;
  
  Wire.beginTransmission( JOY_I2C_ADDR );
  Wire.send( reg_addr );
  Wire.endTransmission();
  
  Wire.requestFrom( JOY_I2C_ADDR, 1 );
  
  while(Wire.available())
    c = Wire.receive();
  
  return c;
}

void write_joy_reg( char reg_addr, char val )
{
  Wire.beginTransmission( JOY_I2C_ADDR );
  Wire.send( reg_addr );
  Wire.send( val );
  Wire.endTransmission();  
}

/* Capacitive touch technique from Mario Becker, Fraunhofer IGD, 2007 http://www.igd.fhg.de/igd-a4 */

char captouched() 
{
  char iii, jjj, retval;
  
  retval = 0;
  
  for( jjj = 0; jjj != 10; jjj++ ) {
    delay( 10 );
  
    pinMode( TOUCH, INPUT );
    digitalWrite( TOUCH, HIGH );
  
    for ( iii = 0; iii <  16; iii++ )
      if( digitalRead( TOUCH ) )
        break;
      
    digitalWrite( TOUCH, LOW );
    pinMode( TOUCH, OUTPUT );
  
    retval += iii;
  }
  
  return retval;
}