#include <Arduino.h>         // required by platformIO
#include <Wire.h>
#include <ESP32Servo.h>

// variable Step motor 1
int dir = 12;
int step = 13;
int counter = 0;
bool condition = false;

// variable Step motor 2
int dir2 = 27;
int step2 = 26;
int counter2 = 0;
bool condition2 = false;

// for servo
const int servoPin1 = 19;
int posServo1 = 30;
Servo servo1;

// variable for sensor
int trigPin = 18;
int echoPin = 32;
int stateUp = 0;
int triggCalled = 0;
int timeFirst;
int timeSecond;
float range = 0.0;
float beforeRange = 0.0;
int flag = 0;

// create time execution for each motor
int timeForMotor1 = 50;
int timeForMotor2 = 50;
int timeForServo = 50;

// create span rotation for motor
float rotateMotor1 = 0.0;
float rotateMotor2 = 0.0;

// create condition for motor to stop
int conditionStopMotor1 = 0;
int conditionStopMotor2 = 0;

int compareTimeGlobal;
int flagInitCompareTime = 0;

// Function For Timer
void stepFun(){
  //check time
  int internalTime = millis();
  
  int diffTime = internalTime - compareTimeGlobal;

  // check if time satisfied or not
  if(diffTime >= timeForMotor1 && conditionStopMotor1 == 0) {
    if(condition){
      digitalWrite(step, HIGH);
      condition = false;
    }else{
      digitalWrite(step, LOW);
      condition = true;
    }
    if (counter >= range) {
      conditionStopMotor1 = 1;
      Serial.println(counter);
    }
    counter++;
    flagInitCompareTime = 0;
    rotateMotor1 = rotateMotor1 + 1.8;
  }

  // check if new position created
  if (range > counter2) {
    conditionStopMotor1 = 0;
  }
}

void stepFun2(){
  //check time
  int internalTime = millis();
  
  int diffTime = internalTime - compareTimeGlobal;

  if(diffTime >= timeForMotor2 && conditionStopMotor2 == 0) {
    if(condition2){
      digitalWrite(step2, HIGH);
      condition2 = false;
    }else{
      digitalWrite(step2, LOW);
      condition2 = true;
    }
    if (counter2 >= range) {
      conditionStopMotor2 = 1;
      Serial.println(counter2);
    }
    counter2++;
    flagInitCompareTime = 0;
    rotateMotor2 = rotateMotor2 + 1.8;
  }

  // check if new position created
  if (range > counter2) {
    conditionStopMotor2 = 0;
  }
}

// create function to move servo
void runServo() {
  //check time
  int internalTime = millis();
  
  int diffTime = internalTime - compareTimeGlobal;

  if(diffTime > timeForServo) {
    servo1.write(posServo1);
    posServo1++;
  }
}

// create function to calculate time for one step for stepper
void calculateTimeForMotor1() {
  Serial.println("check time");
  // get diffrent rotate
  float diffRotate = range - rotateMotor1;
  Serial.println(diffRotate);

  if (diffRotate < 0) {
    conditionStopMotor1 = 1;
  } else {
    // check how many steps neede
    int neededStep = ((int)diffRotate / 1.8) + 1;
    Serial.println(neededStep);

    // check time
    float getTime = 5.0 / (float)neededStep;
    getTime = getTime / 2;
    Serial.println(getTime);
    timeForMotor1 = getTime * 1000;
    Serial.print("time for motor1 : ");
    Serial.println(timeForMotor1);
  }
}

// create function to calculate time for one step for stepper
void calculateTimeForMotor2() {
  Serial.println("check time");
  // get diffrent rotate
  float diffRotate = range - rotateMotor2;

  if (diffRotate < 0) {
    conditionStopMotor2 = 1;
  } else {
    // check how many steps neede
    int neededStep = ((int)diffRotate / 1.8) + 1;

    // check time
    float getTime = 5.0 / (float)neededStep;
    getTime = getTime / 2;
    timeForMotor2 = getTime * 1000;
  }
}

// create function to trigger ultrasonic sensor
void triggerSensor() {
  if(triggCalled == 0) {
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
  }
}

// create function to be called when echo detecting signal as interrupt
void IRAM_ATTR checkEcho(){
  triggCalled = 1;
  beforeRange = range;
  if(stateUp == 0) {
    // first time detecting signal change
    // write current time span
    timeFirst = millis();
    // and set state up to 1
    stateUp = 1;
  } else {
    // second time detecting signal change
    // write current time span
    timeSecond = millis();
    
    // call function
    calculateRange();

    // set state up to default value
    stateUp = 0;
    triggCalled = 0;
  }
}

// create function to be called to calculate range
void calculateRange(){
  // get diffrent time in echo
  unsigned long diffTimeMillis = timeSecond - timeFirst;
  //Serial.println(diffTimeMillis);
  // calculate time in second
  unsigned long timeInSecond = diffTimeMillis * 1000;
  //Serial.println(timeInSecond);
  // get range representation using spee of sound 340 m/s
  range =  timeInSecond / 58;
  // show range
  Serial.print("range : ");
  Serial.println(range);
  Serial.print("before range : ");
  Serial.println(beforeRange);
}

void setup() {
  // Setup Step
  pinMode(dir, OUTPUT);
  pinMode(step, OUTPUT);

  // Setup Step
  pinMode(dir2, OUTPUT);
  pinMode(step2, OUTPUT);

  // setup servo
  servo1.attach(servoPin1, 500, 2400);

  // init pin for sensor
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT_PULLUP);

  // set direction for stepper motor
  digitalWrite(dir, HIGH);
  digitalWrite(dir2, LOW);

  // attach interrupt
  attachInterrupt(echoPin, checkEcho, CHANGE);

  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
}

void loop() {
  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
  triggerSensor();

  if (flagInitCompareTime == 0) {
    compareTimeGlobal = millis();
    flagInitCompareTime = 1;
  }

  if(range != beforeRange) {
    calculateTimeForMotor1();
    calculateTimeForMotor2();
  }

  stepFun();
  stepFun2();
  runServo();
}