#define bleSerial Serial1
#define itrSerial Serial4


const char* cmds[] = {
  "$$$",
  "+\r",
  "V\r",
  "GK\r",
  "GNR\r",
//  "S-,LDsRN4871\r",
//  "SS,C0\r",
  "R,1\r",
//  "hello\r"
};

int idx = 0;


void setup() {
  pinMode(28,OUTPUT);
  pinMode(7,OUTPUT);
  pinMode(6,INPUT);
  digitalWrite(28,HIGH);

  pinMode(3,OUTPUT);
  
  Serial.begin(9600);
  bleSerial.begin(115200,SERIAL_8N1);

  itrSerial.begin(9600);
  
  while(!Serial){}
  while(!bleSerial){}
  while(!itrSerial){}
  
}

int btnPrev = HIGH;

char buff[256];
int bidx = 0;

void loop() {
  

  int btnCurr = digitalRead(6);
  if (btnPrev == HIGH && btnCurr == LOW){
    digitalWrite(7,HIGH);
    delay(500);
    const char* cmd = cmds[idx++];
    int i = 0;
    while (1){
      bleSerial.write(cmd[i]);
      Serial.write(cmd[i]);
      i++;
      if (cmd[i] == 0) break;
    }
    if (idx >= sizeof(cmds)/sizeof(cmds[0])){
      idx = 0;
    }
    digitalWrite(7,LOW);
  }
  
  btnPrev = btnCurr;
  
  if (bleSerial.available() > 0) {
    char b = bleSerial.read();
    char s[2] = {0};
    sprintf(s,"%c",b);
    Serial.print(s);

    if (b == ')'){
      buff[bidx] = b;
      int i;
      for (i = 0; i < 256; i++){
        if (buff[i] == '('){
          i++;
          break;
        }
      }
      int j = i;
      Serial.print("\nSENDING:");
      while (1){
        itrSerial.write(buff[j]);
        Serial.write(buff[j]);
        j++;
        if (buff[j] == ')'){
          break;
        }
      }
      itrSerial.write('\n');
      Serial.print("\n");
      buff[bidx] = 0;
      if (buff[i] == 'm'){
        int n = atoi(buff+i+1);
        int a = ((float)n/100.0) * 2250 + 350;
        int b = 20000-a;
        Serial.print("\nMOTOR:");
        Serial.print(a);
        Serial.print(",");
        Serial.print(b);
        Serial.print("\n");
        for (int i = 0; i < 30; i++){
          digitalWrite(3,HIGH);
          delayMicroseconds(a);
          digitalWrite(3,LOW);
          delayMicroseconds(b);
        }
      }
      if (buff[i] == 'l'){
        if (buff[i+1] == '1'){
          digitalWrite(7,HIGH);
        }else if (buff[i+1] == '0'){
          digitalWrite(7,LOW);
        }
      }
      bidx = 0;
    }else{
      buff[bidx] = b;
      bidx++;
    }
  }
}