#include <XBOXUSB.h>
#include <PS3BT.h>

#define deadzone_xbox 15
#define offset_xbox 7
#define deadzone_ps3 7
#define offset_ps3 2

USB Usb;

XBOXUSB Xbox(&Usb);
PS3BT PS3(&Usb,0x00,0x10,0x60,0x57,0xA0,0x48);

int vals[3];
int pos, negOff, posOff, l2, r2;

void setup() {
  Serial.begin(57600);
  if (Usb.Init() == -1) {
    Serial.print(F("\r\nOSC did not start"));
    while(1);
  }
}

void loop() {
  Usb.Task();
  if(Xbox.Xbox360Connected) {
    calcXboxInput();
  }else if(PS3.PS3Connected) {
    calcPs3Input();
  }
  else{
    vals[0]=127;vals[1]=0;vals[2]=0;
  }

  sendData();
  delay(10);
}

void sendData(){
  Serial.print(";@");
  pad(vals[0]);s
  pad(vals[1]);
  pad(vals[2]);
  Serial.println("@;");
}

void pad(int number){
  for(int i=0; i < 2 - byte(log10(number)); i++)
    Serial.print('0');
  Serial.print(number);
}

void calcXboxInput(){
  pos=Xbox.getAnalogHat(LeftHatX_XBOX)>>8;
  negOff=offset_xbox-deadzone_xbox;
  posOff=offset_xbox+deadzone_xbox;
  l2=Xbox.getButton(L2_XBOX);
  r2=Xbox.getButton(R2_XBOX);

  vals[0]=map(min(pos,negOff),negOff,-128,127,0)+map(max(pos,posOff),posOff,127,0,128);
  vals[1]=((l2>0)? l2: r2);
  vals[2]=((l2>0)? 1: 0);
}

void calcPs3Input(){
  pos=PS3.getAnalogHat(LeftHatX)-128;
  negOff=offset_ps3-deadzone_ps3;
  posOff=offset_ps3+deadzone_ps3;
  l2=PS3.getAnalogButton(L2_ANALOG);
  r2=PS3.getAnalogButton(R2_ANALOG);

  vals[0]=map(min(pos,negOff),negOff,-128,127,0)+map(max(pos,posOff),posOff,127,0,128);
  vals[1]=((l2>0)? l2: r2);
  vals[2]=((l2>0)? 1: 0);
}