#include <ESP32Servo.h>
#define value_square 1
#define value_rectangle 1
Servo myServo;


int value_system_square = 0;
int value_system_rectangle = 0;
int value_system_circle = 0;
int value_system_triangle = 0;
int value_GPIO_input = 0;


int value_total_square = 0 ;
int value_total_rectangle = 0 ;
int value_total_circle = 0 ;
int value_total_triangle = 0 ;
int value_counter = 0 ;

 //unsigned long start_time = millis(); 
const int Full_step = 200;
const int totalSteps = 100;
int microstepDelay_135degree = 300;
int microstepDelay_start = 500;
const int microstepDelay = 1000;

unsigned long previousMillis = 0;
unsigned long interval = microstepDelay;

int value_square_start = 0;
int value_rectangle_start = 0;
int value_circle_start = 0;
int value_triangle_start = 0;
int condition_square = 0;
int condition_rectangle = 0;

int gpio_prin_strp_motorDrive[] = {2, 3, 13, 15, 12};
int gpio_pins_step_motor1[] = {4, 5, 18, 19, 21, 16};
int gpio_pins_step_motor2[] = {25, 26, 27, 32, 33};
int gpio_pins_input[] = {0,2,22,23,17};
void setup() {
  //pinMode(34, INPUT);
  myServo.attach(gpio_pins_step_motor1[5]);
  for (int i = 0; i < 5; i++) {
    REG_WRITE(GPIO_ENABLE_W1TS_REG, (1 << gpio_prin_strp_motorDrive[i]));  
  }
  for (int i = 0; i < 6; i++) {
    REG_WRITE(GPIO_ENABLE_W1TS_REG, (1 << gpio_pins_step_motor1[i]));  
  }
  for (int i = 0; i < 5; i++) {
    gpio_set_direction((gpio_num_t)gpio_pins_step_motor2[i], GPIO_MODE_OUTPUT);
  }
  //gpio_set_direction(GPIO_NUM_34, GPIO_MODE_INPUT);
  for (int i = 0; i < 5; i++){
    REG_WRITE(GPIO_ENABLE_W1TS_REG, (1 << gpio_pins_input[i]));
  }
  Serial.begin(115200);
}
void value_Read_input(){
 
  int square = 0;
  int rectangle = 0;
  int circle = 0;
  int triangle = 0;
  while(true){
  square = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[0]) & 0x01;
  rectangle = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[1]) & 0x01;
  circle = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[2]) & 0x01;
  triangle = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[3]) & 0x01;

  //Serial.print("GPIO pin 13 value: ");
  //Serial.println(square);  
  //Serial.print("value pin 2 value: ");
  //Serial.println(rectangle);
  //Serial.print("GPIO pin 22 value: ");
  //Serial.println(value_circle_start);
  //Serial.print("GPIO pin 23 value: ");
  //Serial.println(value_triangle_start);

  if (square == 1) {
    value_square_start = 1;
    value_total_square++;  
    value_counter++;
    return;
  }
  else if (rectangle == 1) {
    value_rectangle_start = 1;
    value_total_rectangle++;  
    value_counter++;
    return;
  }
  else if (circle == 1) {
    value_circle_start = 1;
    value_total_circle++;
    value_counter++;
    return;  
  }
  else if (triangle == 1) {
    value_triangle_start = 1;
    value_total_triangle++;
    value_counter++;
    return; 
  }
  }
  //Serial.println("dddddddddddddddddddddddddddd");
  
}

void servo_strategy(){
  if(gpio_get_level(GPIO_NUM_34) == 1){
    myServo.write(45);
    if(value_square_start ==1 ){
      myServo.write(80);
    }
  }
   Serial.println(gpio_get_level(GPIO_NUM_34));
}
void step_motor_format_45degree(){
  REG_WRITE(GPIO_OUT_W1TS_REG, (1 << gpio_pins_step_motor1[1]));
  for (int i = 0; i < totalSteps; i++) {
      REG_WRITE(GPIO_OUT_W1TS_REG, (1 << gpio_pins_step_motor1[4]));
      REG_WRITE(GPIO_OUT_W1TS_REG, (1 << gpio_pins_step_motor1[3]));
      delayMicroseconds(microstepDelay);
      REG_WRITE(GPIO_OUT_W1TC_REG, (1 << gpio_pins_step_motor1[3]));
      delayMicroseconds(microstepDelay);
  }
}
void step_motor2_format_45degree(){
  gpio_set_level((gpio_num_t)gpio_pins_step_motor2[1], 1);
  for (int i = 0; i < totalSteps; i++){
    gpio_set_level((gpio_num_t)gpio_pins_step_motor2[3], 1);
    gpio_set_level((gpio_num_t)gpio_pins_step_motor2[4], 1);
    delayMicroseconds(microstepDelay);
    gpio_set_level((gpio_num_t)gpio_pins_step_motor2[4], 0); 
    delayMicroseconds(microstepDelay);
  }
}
void step_motor2_format_135degree(){
  gpio_set_level((gpio_num_t)gpio_pins_step_motor2[1], 1);
  for (int i = 0; i < microstepDelay_135degree; i++){
    gpio_set_level((gpio_num_t)gpio_pins_step_motor2[3], 1);
    gpio_set_level((gpio_num_t)gpio_pins_step_motor2[4], 1);
    delayMicroseconds(microstepDelay_135degree);
    gpio_set_level((gpio_num_t)gpio_pins_step_motor2[4], 0);
    delayMicroseconds(microstepDelay_135degree);
  }
}
void step_motor2_return_45degree(){
  for (int i = 0; i < totalSteps; i++){
    gpio_set_level((gpio_num_t)gpio_pins_step_motor2[3], 0);
    gpio_set_level((gpio_num_t)gpio_pins_step_motor2[4], 1);
    delayMicroseconds(microstepDelay);
    gpio_set_level((gpio_num_t)gpio_pins_step_motor2[4], 0);
    delayMicroseconds(microstepDelay);
  }
}
void step_motor2_return_135degree(){
  for (int i = 0; i < microstepDelay_start; i++){
    gpio_set_level((gpio_num_t)gpio_pins_step_motor2[3], 1);
    gpio_set_level((gpio_num_t)gpio_pins_step_motor2[4], 1);
    delayMicroseconds(microstepDelay_start);
    gpio_set_level((gpio_num_t)gpio_pins_step_motor2[4], 0);
    delayMicroseconds(microstepDelay_start);
  }
}
void step_motor_format_135degree(){
  REG_WRITE(GPIO_OUT_W1TS_REG, (1 << gpio_pins_step_motor1[1]));
  for (int i = 0; i < microstepDelay_135degree; i++) {
      REG_WRITE(GPIO_OUT_W1TS_REG, (1 << gpio_pins_step_motor1[4]));
      REG_WRITE(GPIO_OUT_W1TS_REG, (1 << gpio_pins_step_motor1[3]));
      delayMicroseconds(microstepDelay_135degree);
      REG_WRITE(GPIO_OUT_W1TC_REG, (1 << gpio_pins_step_motor1[3]));
      delayMicroseconds(microstepDelay_135degree);
  }
}
void step_motor_return_45degree(){
  for (int i = 0; i < totalSteps; i++) {
          REG_WRITE(GPIO_OUT_W1TC_REG, (1 << gpio_pins_step_motor1[4]));
          REG_WRITE(GPIO_OUT_W1TS_REG, (1 << gpio_pins_step_motor1[3]));
          delayMicroseconds(microstepDelay);
          REG_WRITE(GPIO_OUT_W1TC_REG, (1 << gpio_pins_step_motor1[3]));
          delayMicroseconds(microstepDelay);
      }
}
void step_motor_return_135degree(){
  for (int i = 0; i < microstepDelay_start; i++){
        REG_WRITE(GPIO_OUT_W1TS_REG, (1 << gpio_pins_step_motor1[3]));
        delayMicroseconds(microstepDelay_start);
        REG_WRITE(GPIO_OUT_W1TC_REG, (1 << gpio_pins_step_motor1[3]));
        delayMicroseconds(microstepDelay_start);
  }
}
void condition_stepmotor1_square_rectangle() {
  int square_condition = 0;
  int rectangle_condition = 0;
  if (value_square_start == 1) {
    step_motor_format_45degree(); 
    unsigned long start_time = millis();
    while (true) {
      square_condition = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[0]) & 0x01;
      rectangle_condition = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[1]) & 0x01;
      Serial.println(square_condition);
      Serial.println(rectangle_condition);
      if (millis() - start_time >= 500 ) {
        unsigned long current_time = millis();
        while(true){
          square_condition = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[0]) & 0x01;
          if(millis() - current_time >= 500){
            step_motor_return_45degree(); 
            value_square_start = 0; 
            Serial.println("condition_square");
            return;
          }
          else if (square_condition == 1){
            unsigned long action_start_time = millis(); 
            while (millis() - action_start_time < 5000) {
            }
              step_motor_return_45degree(); 
              value_square_start = 0; 
              Serial.println("condition_square_Double");
              return;
          }
        }

      }
      else if (rectangle_condition == 1) {
        microstepDelay_135degree = 200;
        step_motor_format_135degree();
        unsigned long current_time = millis();
        while(true){
          rectangle_condition = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[1]) & 0x01;
          if (millis() - current_time >= 500){
            microstepDelay_start = 500;  
            step_motor_return_135degree(); 
            microstepDelay_135degree = 300;
            microstepDelay_start = 500;
            value_square_start = 0;
            Serial.println("condition_square_rectangle");
            return;  
          }
          else if (rectangle_condition == 1){
            unsigned long action_start_time = millis(); 
            while (millis() - action_start_time < 5000) {
            }
              microstepDelay_start = 500;  
              step_motor_return_135degree(); 
              microstepDelay_135degree = 300;
              microstepDelay_start = 500;
              value_square_start = 0;
              Serial.println("condition_square_rectangle_square");
              return;  
          }
        }
      }
    }
  }
  else if (value_rectangle_start == 1){
    step_motor_format_135degree();
    unsigned long start_time = millis();
    while(true){
      square_condition = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[0]) & 0x01;
      rectangle_condition = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[1]) & 0x01;
      if (millis() - start_time >= 500) {
        unsigned long current_time = millis();
        while(true){
          rectangle_condition = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[1]) & 0x01;
          if(millis() - current_time >= 500){
            step_motor_return_135degree();
            value_rectangle_start = 0;
            Serial.println("condition_rectangle");
            return;
          }
          else if (rectangle_condition == 1){
            unsigned long action_start_time = millis(); 
            while (millis() - action_start_time < 5000) {
            }
              step_motor_return_135degree();
              value_rectangle_start = 0;
              Serial.println("condition_rectangle_Double");
              return;
          }
        }
      }
      else if (square_condition == 1){
        step_motor_return_135degree();
        step_motor_format_45degree();
        unsigned long current_time = millis();
        while(true){
          square_condition = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[0]) & 0x01;
          if (millis() - current_time >= 500){
            step_motor_return_45degree();
            value_rectangle_start = 0;
            Serial.println("condition_rectangle_square");
            return;
          }
          else if(square_condition == 1){
            unsigned long action_start_time = millis();
            while(millis() - action_start_time < 5000) {
              step_motor_return_45degree();
              value_rectangle_start = 0;
              Serial.println("condition_square_Double");
              return;
            }
          }
        }
      }
    }
  }
}
void condition_stepmotor2_circle_triangle(){
  int circle_condition = 0;
  int triangle_condition = 0;
  if (value_circle_start == 1){
    step_motor2_format_45degree();
    unsigned long start_time = millis();
    while(true){
      circle_condition = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[2]) & 0x01;
      triangle_condition = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[3]) & 0x01;
      Serial.println(circle_condition);
      Serial.println(triangle_condition);
      if (millis() - start_time >= 500 ) {
        unsigned long current_time = millis();
        while(true){
          circle_condition = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[2]) & 0x01;
          if(millis() - current_time >= 500){
            step_motor2_return_45degree(); 
            value_circle_start = 0; 
            Serial.println("condition_circle");
            return;
          }
          else if (circle_condition == 1){
            unsigned long action_start_time = millis(); 
            while (millis() - action_start_time < 5000) {
            }
            step_motor2_return_45degree(); 
            value_circle_start = 0; 
            Serial.println("condition_circle_Double");
            return;
          }
        }
      }
      else if (triangle_condition == 1) {
          microstepDelay_135degree = 200;
          step_motor2_format_135degree();
          unsigned long current_time = millis();
          while(true){
            triangle_condition = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[3]) & 0x01;
            if (millis() - current_time >= 500){
              microstepDelay_start = 500;  
              step_motor2_return_135degree(); 
              microstepDelay_135degree = 300;
              microstepDelay_start = 500;
              value_circle_start = 0;
              Serial.println("condition_triangle");
              return;  
            }
            else if (triangle_condition == 1){
              unsigned long action_start_time = millis(); 
              while (millis() - action_start_time < 5000) {
              }
                microstepDelay_start = 500;  
                step_motor2_return_135degree(); 
                microstepDelay_135degree = 300;
                microstepDelay_start = 500;
                value_circle_start = 0;
                Serial.println("condition_triangle_Double");
                return;  
            }
          }
      }
    }
  }
  else if (value_triangle_start == 1){
    step_motor2_format_135degree();
    unsigned long start_time = millis();
    while(true){
      circle_condition = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[2]) & 0x01;
      triangle_condition = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[3]) & 0x01;
      if (millis() - start_time >= 500) {
        unsigned long current_time = millis();
        while(true){
          triangle_condition = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[3]) & 0x01;
          if(millis() - current_time >= 500){
            step_motor2_return_135degree();
            value_triangle_start = 0;
            Serial.println("condition_triangle");
            return;
          }
          else if (triangle_condition == 1){
            unsigned long action_start_time = millis(); 
            while (millis() - action_start_time < 5000) {
            }
              step_motor2_return_135degree();
              value_triangle_start = 0;
              Serial.println("condition_triangle_Double");
              return;
          }
        }
      }
      else if (circle_condition == 1){
        step_motor2_return_135degree();
        step_motor2_format_45degree();
        unsigned long current_time = millis();
        while(true){
          circle_condition = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[2]) & 0x01;
          if (millis() - current_time >= 500){
            step_motor2_return_45degree();
            value_triangle_start = 0;
            Serial.println("condition_triangle_circle");
            return;
          }
          else if(circle_condition == 1){
            unsigned long action_start_time = millis();
            while(millis() - action_start_time < 5000) {
              step_motor2_return_45degree();
              value_triangle_start = 0;
              Serial.println("condition_circle_Double");
              return;
            }
          }
        }
      }
    }
  }
}
void step_motor1() {
  while(true){
 unsigned long action_start_time = millis(); 
          while (millis() - action_start_time < 500) {
          }
  Serial.println(value_total_square);
  Serial.println(value_total_rectangle);
  Serial.println(value_total_circle);
  Serial.println(value_total_triangle);
   Serial.println(value_counter);
  
  value_Read_input();
  if(value_counter <= 1){
    condition_stepmotor1_square_rectangle();
    condition_stepmotor2_circle_triangle();
     myServo.write(45);
    
  }
  else if (value_counter >=2) {
     myServo.write(20);
     unsigned long action_start_time = millis(); 
          while (millis() - action_start_time < 500) {
          }
      myServo.write(45);
     condition_stepmotor1_square_rectangle();
    condition_stepmotor2_circle_triangle();
    if(((REG_READ(GPIO_IN_REG) >> gpio_pins_input[4]) & 0x01) == 1 || value_counter == 3 ){
      return;
    }
   
  }
  }
}
//gpio_get_level(GPIO_NUM_34)
void loop() {
  
 // Serial.println((REG_READ(GPIO_IN_REG) >> gpio_pins_input[4]) & 0x01);
  if (((REG_READ(GPIO_IN_REG) >> gpio_pins_input[4]) & 0x01) == 1) {
    value_GPIO_input = 1;
    if (value_GPIO_input == 1) {
      unsigned long Timer_system = millis();
      while (true) {
        value_system_square = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[0]) & 0x01;
        value_system_rectangle = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[1]) & 0x01;
        value_system_circle = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[2]) & 0x01;
        value_system_triangle = (REG_READ(GPIO_IN_REG) >> gpio_pins_input[3]) & 0x01;
        myServo.write(20);
        if (millis() - Timer_system >= 5000) {
          Serial.println("Timeout reached after 5 seconds");
          myServo.write(90);
          value_GPIO_input = 0;
          return;  
        }
        else if (value_system_rectangle == 1 || value_system_square ==1 || value_system_circle == 1 || value_system_triangle == 1 ) {
          myServo.write(45);
          step_motor1();
          Serial.println("Rectangle detected");
          myServo.write(90);
          value_GPIO_input = 0;
          return;
        }    
        unsigned long action_start_time = millis(); 
          while (millis() - action_start_time < 100) {
          }
      }
    }
  }
}