#ifndef DRIVE_CIRCLE_H
#define DRIVE_CIRCLE_H

#include <Arduino.h>

#include "globals.h"

float messung = 0;
unsigned long msPrint = 0;
float degStart = 0;
int oldState = -1;


void initAuto() {
  
}

void driveAuto() {
  int state = 0;
  uint32_t elapsed = millis() - msAuto;
  
  if(elapsed > 1000) state = 1;// fahr los
  if(elapsed > 6000) state = 2; //messung
  if(elapsed > 36000) state = 3; // größerer kreis
  if(elapsed > 41000) state = 4; //messung
  if(elapsed > 71000) state = 5; // größerer kreis
  if(elapsed > 76000) state = 6; //messung
  if(elapsed > 106000) state = 7; //stop
  
  if(state == 2 || state == 4 || state == 6)
    messung = mpu.winkelZ - degStart;

  if(millis() - msPrint > 200) {
    Serial.print("Auto: \t");
    Serial.print(gasVal);
    Serial.print(" \t");
    Serial.print(servoVal);
    Serial.print(" \t");
    Serial.print(elapsed/1000);
    Serial.print("s \tstate=");
    Serial.print(state);
    Serial.print(" \t");
    Serial.print(mpu.winkelZ);
    Serial.print("° \t");
    Serial.print(messung);
    Serial.print("° \t");
    Serial.print(messung / 30);
    Serial.print("°/s \t");
    Serial.println();

    Serial3.print("Auto: \t");
    Serial3.print(gasVal);
    Serial3.print(" \t");
    Serial3.print(servoVal);
    Serial3.print(" \t");
    Serial3.print(elapsed/1000);
    Serial3.print("s \tstate=");
    Serial3.print(state);
    Serial3.print(" \t");
    Serial3.print(mpu.winkelZ);
    Serial3.print("° \t");
    Serial3.print(messung);
    Serial3.print("° \t");
    Serial3.print(messung / 30);
    Serial3.print("°/s \t");
    Serial3.println();
    msPrint = millis();
  }


  if(oldState == state) {
    return;
  }

  float spd = 0.11;
  
  switch(state) {
    case 0:
      setMotor(0);
      setServo(0);
      break;
    case 1:
      setMotor(spd);
      setServo(1);
      break;
    case 2:
      setMotor(spd);
      setServo(1);
      degStart = mpu.winkelZ;
      break;
    case 3:
      setMotor(spd);
      setServo(0.8);
      break;
    case 4:
      setMotor(spd);
      setServo(0.8);
      degStart = mpu.winkelZ;
      break;
    case 5:
      setMotor(spd);
      setServo(0.6);
      break;
    case 6:
      setMotor(spd);
      setServo(0.6);
      degStart = mpu.winkelZ;
      break;
    case 7:
      setMotor(0);
      setServo(0);
      break;
  }  
  oldState = state;
}

#endif