#include <Wire.h>

#define SensorPin 8
#define RelayPin 5

bool SensorState = LOW;
bool SensorLastState = HIGH;

unsigned long T[5] = {0, 0, 0, 0, 0};
unsigned long T_start = 0;
unsigned long T_stop = 0;

unsigned long CalibStart = 0;
unsigned long CalibStop = 0;
unsigned long CalibTime = 0;

int Tindex = 0;

void setup() 
{
  pinMode(SensorPin, INPUT);
  pinMode(RelayPin, OUTPUT);

  digitalWrite(RelayPin, LOW);

  Wire.begin();
  Serial.begin(9600);
}

void loop() 
{
  Serial.println("Oczekiwanie na polecenie:");
  Serial.println("- \"kalibracja\"...");
  Serial.println("- \"pozycja 1\"...");
  Serial.println("- \"pozycja 2\"...");
  Serial.println("- \"pozycja 3\"...");
  Serial.println("- \"pozycja 4\"...");
  Serial.println("- \"pozycja 5\"...");  

  while (true) 
  {
    if (Serial.available() > 0) 
    {
      String command = Serial.readStringUntil('\n');
      command.trim();

      if (command.equals("kalibracja")) 
      {
        Serial.println("Rozpoczęcie kalibracji...");
        Calibration();
        Serial.println("Kalibracja zakończona.");
        break;
      }
      else if (command.equals("pozycja 1")) 
      {
        Serial.println("Pozycja 1...");
        Rotate_To_Position(0);
        Serial.println("Pozycja 1 osiągnięta");
        break;
      }
      else if (command.equals("pozycja 2"))
      {
        Serial.println("Pozycja 2...");
        Rotate_To_Position(1);
        Serial.println("Pozycja 2 osiągnięta");
        break;
      }
      else if (command.equals("pozycja 3"))
      {
        Serial.println("Pozycja 3...");
        Rotate_To_Position(2);
        Serial.println("Pozycja 3 osiągnięta");
        break;
      }
      else if (command.equals("pozycja 4"))
      {
        Serial.println("Pozycja 4...");
        Rotate_To_Position(3);
        Serial.println("Pozycja 4 osiągnięta");
        break;
      }
      else if (command.equals("pozycja 5"))
      {
        Serial.println("Pozycja 5...");
        Rotate_To_Position(4);
        Serial.println("Pozycja 5 osiągnięta");
        break;
      }
    }
  }
}

void Calibration()
{
  Tindex = 0;
  T_start = 0;
  T_stop = 0;

  for (int i = 0; i < 5; i++) 
  {
    T[i] = 0;
  }

  CalibStart = 0;
  CalibStop = 0;
  CalibTime = 0;
  CalibStart = millis();

  while(Tindex < 5)
  {
    TimeArray();
    digitalWrite(RelayPin, HIGH);
  }

  digitalWrite(RelayPin, LOW);
}

void TimeArray() 
{
  Serial.println(Tindex);
  SensorState = digitalRead(SensorPin);

  if (SensorState != SensorLastState)
  {
    if (SensorState == LOW) 
    {
      T_start = millis();
    }
    else if (SensorState == HIGH) 
    {
      T_stop = millis();
  
      T[Tindex] = T_stop - T_start;
      Tindex++;

      if (Tindex >= 5)
      {
        for (int i = 0; i < 5; i++) 
        {
          Serial.print("T");
          Serial.print(i + 1);
          Serial.print(" = ");
          Serial.print(T[i]);
          Serial.println(" ms");
        }

        CalibStop = millis();
        CalibTime = CalibStop - CalibStart;
        Serial.print("Czas kalibracji = ");
        Serial.print(CalibTime);
        Serial.println(" ms");
      }
    }

    SensorLastState = SensorState;
  }
}

void Rotate_To_Position(int targetIndex)
{
  bool positionFound = false;
  while (!positionFound)
  {
    digitalWrite(RelayPin, HIGH);
    unsigned long TimeCurrent = 0;
    SensorState = digitalRead(SensorPin);

    if (SensorState == LOW) 
    {
      T_start = millis();
      while(digitalRead(SensorPin) == LOW);
      T_stop = millis();
      TimeCurrent = T_stop - T_start;
    }

    if (abs(TimeCurrent - T[targetIndex]) <= 100) 
    {
      positionFound = true;
    }
  }

  digitalWrite(RelayPin, LOW);
}