#define BLEcacheMediaKey 16
#define BLEcacheGamepadbtns 14
#define BLEcacheHAT 4
#define BLEcacheMouse 5
#define BLEcacheKeyboard 255
#define BLEAnalog 12

byte BLEBTNcacheMediaKey[BLEcacheMediaKey];
byte BLEBTNcacheGamepadbtns[BLEcacheGamepadbtns];
byte BLEBTNcacheHAT[BLEcacheHAT];
byte BLEBTNcacheMouse[BLEcacheMouse];
byte BLEBTNcacheKeyboard[BLEcacheKeyboard];

int16_t BLEAnalogSet[BLEAnalog];
int32_t BLEAnalogincrement[BLEAnalog];

String BLEProgramCache[5];
int BLEProgramActive = 0;

void iniBLE(bool first = true){
  for (int i = 0; i < BLEcacheMediaKey; i++) {
    BLEBTNcacheMediaKey[i] = 0;
  }
  for (int i = 0; i < BLEcacheGamepadbtns; i++) {
    BLEBTNcacheGamepadbtns[i] = 0;
  }
  for (int i = 0; i < BLEcacheHAT; i++) {
    BLEBTNcacheHAT[i] = 0;
  }
  for (int i = 0; i < BLEcacheMouse; i++) {
    BLEBTNcacheMouse[i] = 0;
  }
  for (int i = 0; i < BLEcacheKeyboard; i++) {
    BLEBTNcacheKeyboard[i] = 0;
  }
  for (byte i = 0; i < BLEAnalog; i++) {
    BLEAnalogSet[i] = 0;
    BLEAnalogincrement[i] = 0;
  }
  if(first)blex.begin();
  loadProgram(BLEProgramCache[BLEProgramActive]);
}

/*
[1] //scancode
  [0-255]
[2] //MediaKey
  [0-15]
[3] //Gamepadbtns
  [0-13]
[4] //HAT
  [0-3]
[5] //Mouse
 [0-4]
 */

void BLEpress(byte type, byte btn){
  if(type == 1){ //scancode [0-255]
    if(BLEBTNcacheKeyboard[btn]<255)BLEBTNcacheKeyboard[btn]++;
    blex.press(btn);
  }
  if(type == 2){ //MediaKey [0-15]
    if(BLEBTNcacheMediaKey[btn]<255)BLEBTNcacheMediaKey[btn]++;
  }
  if(type == 3){ //Gamepadbtns [0-13]
    if(BLEBTNcacheGamepadbtns[btn]<255)BLEBTNcacheGamepadbtns[btn]++;
  }
  if(type == 4){ //HAT [0-3]
    if(BLEBTNcacheHAT[btn]<255)BLEBTNcacheHAT[btn]++;
  }
  if(type == 5){ //Mouse[0-4]
    if(BLEBTNcacheMouse[btn]<255)BLEBTNcacheMouse[btn]++;
  }
}
void BLErelese(byte type, byte btn){
  if(type == 1){ //scancode [0-255]
    if(BLEBTNcacheKeyboard[btn]>0)BLEBTNcacheKeyboard[btn]--;
    if(BLEBTNcacheKeyboard[btn]==0)blex.release(btn);
  }
  if(type == 2&&btn<=15){ //MediaKey [0-15]
    if(BLEBTNcacheMediaKey[btn]>0)BLEBTNcacheMediaKey[btn]--;
  }
  if(type == 3&&btn<=13){ //Gamepadbtns [0-13]
    if(BLEBTNcacheGamepadbtns[btn]>0)BLEBTNcacheGamepadbtns[btn]--;
  }
  if(type == 4&&btn<=3){ //HAT [0-3]
    if(BLEBTNcacheHAT[btn]>0)BLEBTNcacheHAT[btn]--;
  }
  if(type == 5&&btn<=4){ //Mouse[0-4]
    if(BLEBTNcacheMouse[btn]>0)BLEBTNcacheMouse[btn]--;
  }
}
void BLEsetAxis(byte axe, int16_t value){
  BLEAnalogSet[axe] = value;
}
void BLEincreseAxis(byte axe, int16_t value){
  int32_t now = BLEAnalogincrement[axe];
  now+=(int32_t) value;
  if(now>65536)now = 65536;
  if(now<-65536)now = -65536;
  NRFAnalogincrement[axe] = (int32_t) now;
}

int16_t BLEcalcAxis(byte i){
  int32_t now = (int32_t)BLEAnalogSet[i];
  now+=(int32_t) BLEAnalogincrement[i];
  if(now>32767)now = 32767;
  if(now<-32767)now = -32767;
  //if(NRFAnalogSet[i] == 0){
    NRFAnalogincrement[i] = 0;
  //}
  NRFAnalogSet[i] = 0;
  return (int16_t) now;

}

uint16_t mediaLast = 0;
int16_t BlEAnalogCache[BLEAnalog];

void BLEsend(){
  uint8_t mbuttons = 0;
  uint16_t k_16 = 0;
  /*for (byte i = 0; i < BLEcacheKeyboard; i++) {
    if(BLEBTNcacheKeyboard[i]>0){

    }
  }*/
  for (byte i = 0; i < BLEcacheMediaKey; i++) {
    if(BLEBTNcacheMediaKey[i]>0){
      k_16 |= (uint16_t)1<<i;
      //BLEBTNcacheMediaKey[i] = 0;
    }
  }
  for (byte i = 0; i < BLEcacheGamepadbtns; i++) {
    if(BLEBTNcacheGamepadbtns[i]>0)blex.gpress((uint16_t)1<<i,false);
    else blex.grelease((uint16_t)1<<i,false);
  }
  for (byte i = 0; i < BLEcacheHAT; i++) {
    if(BLEBTNcacheHAT[i]>0)blex.pressHat(i,false);
    else blex.releaseHat(i,false);
  }
  for (byte i = 0; i < BLEcacheMouse; i++) {
    if(BLEBTNcacheMouse[i]>0){
      mbuttons |= 1<<i;
    }
  }
  //lx, ly, rx, ry, lt, rt, lr, rr, mx, my, sm, sh
  blex.setAxes(BLEcalcAxis(0),BLEcalcAxis(1),BLEcalcAxis(2),BLEcalcAxis(3),BLEcalcAxis(4),BLEcalcAxis(5),false);
  BLEcalcAxis(6);
  BLEcalcAxis(7);

  blex.move(
    max(min((float)BLEcalcAxis(8)*currentTime/1000.0/10.0,127.0),-127.0),
    max(min((float)-BLEcalcAxis(9)*currentTime/1000.0/10.0,127.0),-127.0),
    max(min((float)BLEcalcAxis(10)*currentTime/1000.0/200.0,127.0),-127.0),
    max(min((float)BLEcalcAxis(11)*currentTime/1000.0/200.0,127.0),-127.0),
    mbuttons);
  if(k_16!=mediaLast){
    mediaLast = k_16;
    MediaKeyReport _mediaKeyReport;
    _mediaKeyReport[0] = (uint8_t)((k_16 & 0xFF00) >> 8);
    _mediaKeyReport[1] = (uint8_t)(k_16 & 0x00FF);
    blex.sendReport(&_mediaKeyReport);
  }
  blex.sendGamepadReport();
  leds[21] = blex.isConnected() ? BLEColorConnected : BLEColorNotConnected;
}

void BLEnextProgram(){
  BLEProgramActive++;
  if(BLEProgramActive>4)BLEProgramActive = 0;
  if(BLEProgramActive<0)BLEProgramActive = 0;
  while(BLEProgramActive!=0&&BLEProgramCache[BLEProgramActive].length()){
    BLEProgramActive++;
    if(BLEProgramActive>4)BLEProgramActive = 0;
  }
  loadProgram(BLEProgramCache[BLEProgramActive]);
}

void BLEclick(){
  BLEnextProgram();
}