const int potPinOne = 14;
const int potPinTwo = 15;
const int ledPin = 13;
const int buttonPin = 10;

int steerOne = 0;
int steerTwo = 0;

String incomingByte;

int basis = 250;
double distance1 = 0;
double distance2 = 0;
double tempDistance1 = 0;
double tempDistance2 = 0;
unsigned long timerPauzeCheck = 0;
boolean paused1 = false;
boolean paused2 = false;
boolean gasActive1 = false;
boolean gasActive2 = false;
unsigned long timer1 = 0;
unsigned long timer2 = 0;


void setup() {
  Serial.begin(9600);
  pinMode(ledPin, OUTPUT);
  pinMode(buttonPin, INPUT_PULLUP);
}

void loop() {
  steerOne = analogRead(potPinOne);
  steerTwo = analogRead(potPinTwo);
  Joystick.X(steerOne * 1.1);
  Joystick.Y(steerTwo * 1.1);


  incomingByte = Serial.readString();

  //Serial.println(incomingByte);

  int paceOne = getValue(incomingByte, ',', 0).toInt();
  int paceTwo = getValue(incomingByte, ',', 1).toInt();

  gas(paceOne, basis, timer1, paused1, gasActive1, 1, 1);
  gas(paceTwo, basis, timer2, paused2, gasActive2, 2, 2);

}

void gas(int pace, int basis, unsigned long timer, boolean paused, boolean gasActive, int knop, int ergometer) {
  Serial.println("executing gas");
  if (pace != 0 && millis() > timer) {
    double pauze;
    if (!gasActive && !paused) {
      pauze = pow(((double)basis / (double)pace), 2) * 1000.0;
      Joystick.button(knop, 1);
      gasActive = true;
    }
    else {
      pauze = pow(((double)pace / (double)basis), 2) * 1000.0;
      Joystick.button(knop, 0);
      gasActive = false;
    }
    timer = millis() + pauze;
    if (ergometer == 1) {
      gasActive1 = gasActive;
      timer1 = timer;
    }
    else if (ergometer == 2) {
      gasActive2 = gasActive;
      timer2 = timer;
    }
  }
}

String getValue(String data, char separator, int index)
{
  int found = 0;
  int strIndex[] = {0, -1};
  int maxIndex = data.length() - 1;

  for (int i = 0; i <= maxIndex && found <= index; i++) {
    if (data.charAt(i) == separator || i == maxIndex) {
      found++;
      strIndex[0] = strIndex[1] + 1;
      strIndex[1] = (i == maxIndex) ? i + 1 : i;
    }
  }

  return found > index ? data.substring(strIndex[0], strIndex[1]) : "";
}