#include <Wire.h>
#include <Servo.h>

#define SLAVE_ADDRESS 0x04

double erg = 0.0;
double maximum;
byte Array[4];
byte Arr[6];
int A[4];
int p = 0;
int s;
double si = 0.0;
int W = 0;
int i = 0;
int Turn;

Servo tServo;

int d = 2;
int k = 87;
int b = k - 45;
int HallOa = 16;
int HallTa = 17;
int HallOb = 18;
int HallTb = 19;

int dira = 4;
int dirb = 7;
int Speeda = 5;
int Speedb = 6;

int Entfg;
int Winkel;

volatile int Counta = 0;
volatile int Countb = 0;

void setup() {
  Serial.begin(38400);

  tServo.attach(23);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(7, OUTPUT);

  pinMode(HallOa, INPUT_PULLUP);
  pinMode(HallTa, INPUT_PULLUP);
  pinMode(HallOb, INPUT_PULLUP);
  pinMode(HallTb, INPUT_PULLUP);

  Wire.begin(SLAVE_ADDRESS);

  Wire.onReceive(receiveData);
  Wire.onRequest(sendData);

  attachInterrupt(digitalPinToInterrupt(HallOa), Reada, CHANGE);
  attachInterrupt(digitalPinToInterrupt(HallTa), Reada, CHANGE);
  attachInterrupt(digitalPinToInterrupt(HallOb), Readb, CHANGE);
  attachInterrupt(digitalPinToInterrupt(HallTb), Readb, CHANGE);
  
      Array[0] = 0;
      Array[1] = 0;
      Array[2] = 0;
      Array[3] = 0;
}

void loop() {
  switch (p) {

    case 1:

      s = 1;
      Array[1] = 0;
      Array[0] = 0;
      tServo.write(b);
      erg = 0.0;
      for (int i = 0; i <= 99; i++) {
        erg = erg + analogRead(9);
      }
      erg = erg / 100.0;
      while (erg > 255.0) {
        erg = erg - 255.0;
        Array[1] = Array[1] + 1;
      }
      Array[0] = erg;
      Array[2] = b;
      b++;
      if (b == k + 46) {
        b = k - 45;
      }
      p = 0;
      break;

    case 2:
      s = 2;
      digitalWrite(dira, LOW);
      digitalWrite(dirb, LOW);
      analogWrite(Speeda, A[1]); // links
      analogWrite(Speedb, A[2]); // rechts
      /*while (Entfg > Counta or Entfg > Countb) {
        Array[0] = 0;
        }
        if (Counta >= Entfg and Countb >= Entfg) {
        Array[0] = ((Counta / 2) + (Countb / 2) / 28);
        p = 0;
        }*/
      p = 0;
      break;

    case 3:
      s = 3;
      Counta = 0;
      Countb = 0;
      Entfg = 335;
      digitalWrite(dira, HIGH);
      digitalWrite(dirb, LOW);
      analogWrite(Speeda, 90);
      analogWrite(Speedb, 90);
      while (Counta < Entfg || Countb < Entfg) {
        Entfg = 335;
      }
      p = 0;
      break;

    case 4:
      s = 4;
      digitalWrite(dira, LOW);
      digitalWrite(dirb, LOW);
      analogWrite(Speeda, 0);
      analogWrite(Speedb, 0);
      //Serial.println(s);
      p = 0;
      break;
    case 5:
      Counta = 0;
      Countb = 0;
      Array[1] = 0;
      Array[0] = 0;
      Array[2] = 0;
      Array[3] = 0;
      p = 0;
   case 6:
      s = 6;
      Counta = 0;
      Countb = 0;
      Entfg = A[3];
      digitalWrite(dira, A[1]);
      digitalWrite(dirb, A[2]);
      analogWrite(Speeda, 90);
      analogWrite(Speedb, 90);
      while (Counta < Entfg || Countb < Entfg) {
        Entfg = A[3];
      }
      p = 0;
      break;
    case 7:
      s = 7;
      Counta = 0;
      Countb = 0;
      Entfg = A[3]*2;
      digitalWrite(dira, A[1]);
      digitalWrite(dirb, A[2]);
      analogWrite(Speeda, 90);
      analogWrite(Speedb, 90);
      while (Counta < Entfg || Countb < Entfg) {
        Entfg = A[3]*2;
      }
      p = 0;
      break;
     case 8:
      s = 8;
      
      Counta = 0;
      Countb = 0;
      digitalWrite(dira, HIGH);
      digitalWrite(dirb, LOW);
      analogWrite(Speeda, 90);
      analogWrite(Speedb, 90);
      Array[0] = 0;
      Array[1] = 0;
      Entfg = 335/90*A[1];
      digitalWrite(dira, HIGH);
      digitalWrite(dirb, LOW);
      analogWrite(Speeda, 90);
      analogWrite(Speedb, 90);
      while (Counta < Entfg || Countb < Entfg) {
        Entfg = 335/90*A[1];
      }
      p = 0;
     break;
     default:
      delay(10);
      Serial.println("normal ");
      break;
     delay(10);
     Serial.println("happy birthday");
     

  }
}

void receiveData(int byteCount) {
  while (Wire.available()) {
    byte ar = Wire.read();
    A[i] = ar;
    i++;
  }
  i = 0;
  p = A[0];
}

void sendData() {
  
    //Serial.print(" s ");
    //Serial.println(s);
  switch (s) {
    case 1:
    Serial.print("Entfernung: ");
    Serial.print(Array[0]+Array[1]*255);
    Serial.print(" Winkel: ");
    Serial.println(Array[2]);
      Wire.write(Array, 3);
      Array[1] = 0;
      Array[0] = 0;
      Array[2] = 0;
      Array[3] = 0;
      s = 0;
      break;

    case 2:
      Array[1] = 0;
      Array[0] = 0;
      Array[2] = 0;
      Array[3] = 0;
      //Serial.println("222");
      while (Counta > 255) {
        Array[1] = Array[1] + 1;
        Counta = Counta - 255;
      }
      Array[0] = Counta;
      while (Countb > 255) {
        Array[3] = Array[3] + 1;
        Countb = Countb - 255;
      }
      Array[2] = Countb;
      Countb = 0;
      Counta = 0;
      Wire.write(Array, 4);
      s = 0;
      /*Serial.print("Counta ");
      Serial.print(Array[0] + Array[1] * 255);
      Serial.print(" Countb ");
      Serial.println(Array[2] + Array[3] * 255);*/
      Wire.write(Array, 4);
      break;

    case 3:
      Array[0] = 0;
      Array[1] = 0;
      Array[2] = 0;
      Array[3] = 0;
      Counta = 0;
      Countb = 0;
      s = 0;
      break;

    case 4:
      //Serial.print("444");
      Array[1] = 0;
      Array[0] = 0;
      Array[2] = 0;
      Array[3] = 0;
      while (Counta > 255) {
        Array[1] = Array[1] + 1;
        Counta = Counta - 255;
      }
      Array[0] = Counta;
      while (Countb > 255) {
        Array[3] = Array[3] + 1;
        Countb = Countb - 255;
      }
      Array[2] = Countb;
      Countb = 0;
      Counta = 0;
      /*Serial.print("Counta ");
      Serial.print(Array[0] + Array[1] * 255);
      Serial.print(" Countb ");
      Serial.println(Array[2] + Array[3] * 255);*/
      Wire.write(Array, 4);
      
      s = 0;
      break;

    default:
    
      Array[1] = 0;
      Array[0] = 0;
      Array[2] = 0;
      Array[3] = 0;
      Wire.write(Array, 4);
      break;
      
      case 8:
      s = 0;
      break;
  }
}

void Reada () {
  Counta++;
  if (s != 2) {
    if (Counta >= Entfg) {
      analogWrite(Speeda, 0);
    }
  }
}


void Readb () {
  Countb++;
  if (s != 2 and s != 8) {
    if (Countb >= Entfg) {
      analogWrite(Speedb, 0);
    }
  }
}
