#include <XBOXUSB.h>
#include <PS3BT.h>
#include <Wire.h>
#include <LCD.h>
#include <LiquidCrystal_I2C.h>

#define LCD_UPDATE 50
#define TX_INTERVAL 10

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

LiquidCrystal_I2C   lcd(0x27,2,1,0,4,5,6,7);

USB Usb;

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

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

String buffout="";
String oldbuffout="";

String inStr;
char buff[15];
char inChar;

float batt[4];
boolean cutoff=false;
int temp;

int index;
int nextIndex;

int lcdTimer;
double old_tx;

void initLCD(){
  lcd.begin (16,2);
  lcd.setBacklightPin(3,POSITIVE);
  lcd.setBacklight(HIGH);
  lcd.print("Starting...");
}

/*----------------------------------------Main loop--------------------------------------*/

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

void loop() {
  Usb.Task();
  getControllerInputs();
  sendData();
  parseData();
  displayLCD();
  //delay(10);
}

/*------------------------------------Main Functions-----------------------------------------*/

void getControllerInputs(){
  if(Xbox.Xbox360Connected) {
    calcXboxInput();
  }else if(PS3.PS3Connected) {
    calcPs3Input();
  }
  else{
    vals[0]=127;vals[1]=0;vals[2]=0,vals[3]=0;
  }
}

void sendData(){
  if(millis()>=(old_tx+TX_INTERVAL)){
    oldbuffout=buffout;
    buffout="";
    buffout+=pad(vals[0]);
    buffout+=pad(vals[1]);
    buffout+=pad(vals[2],2);
    buffout+=vals[3];
    buffout+="\n";
    if(oldbuffout!=buffout){
      Serial.print(buffout);
    }
    Serial1.print(buffout);
    old_tx=millis();
  }
}

/*-------------------------------------Sub functions--------------------------------------*/

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);
  vals[3]=Xbox.getButton(B);
}

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);
  vals[3]=PS3.getButton(CIRCLE);
}

String pad(int number){
  return pad(number,3);
}
String pad(int number,int digits){
  String ret="";
  for(int i=0; i < (digits-1) - byte(log10(number)); i++)
    ret+='0';
  ret+=number;
  return ret;
}


void parseData(){
  if(Serial1.available()>17){
    inStr="";
    do{
      inChar=Serial1.read();
      inStr+=inChar;
    }while(inChar!='\n');
    if(inStr.length()==18){
      inStr.substring(0,3).toCharArray(buff,4);
      batt[0]=atoi(buff)/100.00;
      inStr.substring(3,6).toCharArray(buff,4);
      batt[1]=atoi(buff)/100.00;
      inStr.substring(6,9).toCharArray(buff,4);
      batt[2]=atoi(buff)/100.00;
      inStr.substring(9,12).toCharArray(buff,4);
      batt[3]=atoi(buff)/100.00;
      inStr.substring(12,13).toCharArray(buff,2);
      cutoff=atoi(buff);
      inStr.substring(13,17).toCharArray(buff,5);
      temp=atoi(buff);
    }
  }
}

void displayLCD(){
  //Serial.println(lcdTimer);
  if(lcdTimer==LCD_UPDATE){
    lcd.clear();
    //lcd.home();
    lcd.setCursor( 0, 0);
    lcd.print(batt[0]);
    lcd.setCursor( 5, 0);
    lcd.print(batt[1]);
    lcd.setCursor( 0, 1);
    lcd.print(batt[2]);
    lcd.setCursor( 5, 1);
    lcd.print(batt[3]);
    lcd.setCursor( 10, 0);
    lcd.print(temp);
    lcd.setCursor( 10, 1);
    lcd.print(((cutoff)? "STOP!!":"Safe"));
    lcdTimer=0;
  }
  lcdTimer++;
}