#include <AltSoftSerial.h>

AltSoftSerial Bluetooth;

int incoming;
int data[4] {0,0,0,0};
byte i = 0;

unsigned long HuidigMillis, StartMillis, VerlopenMillis, minuten, seconden, milliseconden;      // Tijds variabelen

float snelheid;

float topspeed = 0;
float verbruikt = 0;

int Run = 0;
int Auto = 0;
int X_as = 0;
int Y_as = 0;

void stopwatch(void) {
  HuidigMillis = millis();
  VerlopenMillis = (HuidigMillis - StartMillis);

  milliseconden = (VerlopenMillis % 1000);
  seconden = ((VerlopenMillis / 1000) % 60);
  minuten = ((VerlopenMillis / 60000) % 60);
}

void receive3Bytes(void)
{
  const byte aantal = 2;
  if (Bluetooth.available() > 0) {
    incoming = Bluetooth.read();
    if (incoming == 200) {
      i = 0;
      for (i; i <= 1; i++) {
      incoming = Bluetooth.read();
      data[i] = incoming;
      i++;
      }
    }
    else if (incoming == 255) {
      i = 2;
      for (i; i <= 3; i++) {
      incoming = Bluetooth.read();
      data[i] = incoming;
      i++;
      }
    }
    Run = data[0];
    Auto = data[1];
    X_as = data[2];
    Y_as = data[3];    
  }
}



void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Bluetooth.begin(9600);
  Serial.println("Welkom bij de AGV, dit is een kleine demo van de wireless communicatie");
  Serial.println("Ik wacht op het inklikken van de run button...");
  delay(2000);
  while (Bluetooth.available() == 0) {
    
  }
  receive3Bytes();
  Serial.println("Mijn status is als volgt:");
  Serial.print("Run: ");
  Serial.print(Run);
  Serial.print("      Auto: ");
  Serial.print(Auto);
  Serial.println();
  Serial.println();
  
  StartMillis = millis();
}

void loop() {  
  switch (Run) {
    case 0:                                                   // Robot is gestopt
      StartMillis = millis();
      break;

    case 1:                                                   // Robot is gestart
      stopwatch();
      verbruikt = verbruikt + random(10.0,30.0);

      switch (Auto) {
        case 0:                                               // Robot staat in auto modus
          Serial.print(Run);
          Serial.print(",   ");
          Serial.print(Auto);
          Serial.print(",   ");
          Serial.print(X_as);
          Serial.print(",   ");
          Serial.print(Y_as);
          Serial.println();            
          break;

        case 1:                                               // Robot staat in manual modus
          snelheid = random(1.0,15.0);
          if (snelheid > topspeed) {
            topspeed = snelheid;
          }
          Bluetooth.print(topspeed);
          Bluetooth.print(" m/s");
          Bluetooth.print("|");
          Bluetooth.print(minuten);
          Bluetooth.print(":");
          Bluetooth.print(seconden);
          Bluetooth.print(".");
          Bluetooth.print(milliseconden);
          Bluetooth.print("|");
          Bluetooth.print(verbruikt);
          Bluetooth.print(" W");
          Bluetooth.println();
          break;
      }
      break;
  }

  receive3Bytes();
}