#include <AltSoftSerial.h>

AltSoftSerial Bluetooth;

byte incoming;
byte value;
byte 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;

byte Run = 0;
byte Auto = 0;
byte X_as = 0;
byte Y_as = 0;

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

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

void receive5Bytes(void)
{
  if (Bluetooth.available() > 0) 
  {
    incoming = Bluetooth.read();
    while (incoming == 250) 
    {
      for (i = 0; i <= 3; i++)
      { 
        delayMicroseconds(10);
        value = Bluetooth.read();
        
        data[i] = value;
        i++;
      }
      Run = data[0];
      Auto = data[1];
      X_as = data[2];
      Y_as = data[3];
      break;
    }   
  }
}



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) 
  {
    
  }
  
  receive5Bytes();
  
  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() {
  Serial.print("Run: ");
  Serial.print(Run);
  Serial.print(",   ");
  Serial.print("Auto: ");
  Serial.print(Auto);
  Serial.println();  
  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(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;
  }

  receive5Bytes();
}