#include <Arduino.h>
#include <Wire.h>
#include <MPU6050_tockn.h>
TaskHandle_t TareaProcesador1;
void loop1(void *pvParameters);
volatile int left_motor, throttle_left_motor, throttle_counter_left_motor, throttle_left_motor_memory;
volatile int right_motor, throttle_right_motor, throttle_counter_right_motor, throttle_right_motor_memory;
volatile int dir = 0;
bool FWD = HIGH;
bool BWD = LOW;

#define STEP_1 13
#define DIR_1 25
#define STEP_2 33
#define DIR_2 14
MPU6050 mpu6050(Wire);

float ax, ay, az;
long timer = 0;
void setup()
{
  pinMode(23, OUTPUT);
  pinMode(STEP_1, OUTPUT);
  pinMode(STEP_2, OUTPUT);
  pinMode(DIR_1, OUTPUT);
  pinMode(DIR_2, OUTPUT);
  digitalWrite(DIR_1, LOW);
  digitalWrite(DIR_2, LOW);
  Wire.begin();
  Serial.begin(115200);
  Wire.begin();
  xTaskCreatePinnedToCore(loop1, "Tarea1", 10000, NULL, 1, &TareaProcesador1, 0);

  delay(2000); // Pause for 2 seconds

  mpu6050.begin();
  mpu6050.calcGyroOffsets(false);
}

void loop()
{
  mpu6050.update();
  if (millis() - timer > 10)
  {

    // ax = mpu6050.getAngleX();
    ay = mpu6050.getAngleY();
    //az = mpu6050.getAngleZ();

    /*   Serial.print(ax), Serial.print(",");
    Serial.print(ay), Serial.print(",");
    Serial.println(az);*/
    if (ay > 4)
    {
      dir = -1;
    }
    else if (ay < -4)
    {
      dir = 1;
    }
    else
    {
      dir = 0;
    }
    throttle_left_motor = 6 * dir;
    throttle_right_motor = 6 * dir;

    timer = millis();
  }
}

void loop1(void *pvParameters)
{
  for (;;)
  {

    throttle_counter_left_motor++; //Increase the throttle_counter_left_motor variable by 1 every time this routine is executed
    if (throttle_counter_left_motor > throttle_left_motor_memory)
    {                                                   //If the number of loops is larger then the throttle_left_motor_memory variable
      throttle_counter_left_motor = 0;                  //Reset the throttle_counter_left_motor variable
      throttle_left_motor_memory = throttle_left_motor; //Load the next throttle_left_motor variable
      if (throttle_left_motor_memory < 0)
      {                                   //If the throttle_left_motor_memory is negative
        digitalWrite(DIR_1, BWD);         //Set output 3 low to reverse the direction of the stepper controller
        throttle_left_motor_memory *= -1; //Invert the throttle_left_motor_memory variable
      }
      else
        digitalWrite(DIR_1, FWD); //Set output 3 high for a forward direction of the stepper motor
    }
    else if (throttle_counter_left_motor == 1)
      digitalWrite(STEP_1, HIGH); //Set output 2 high to create a pulse for the stepper controller
    else if (throttle_counter_left_motor == 2)
      digitalWrite(STEP_1, LOW); //Set output 2 low because the pulse only has to last for 20us

    //right motor pulse calculations
    throttle_counter_right_motor++; //Increase the throttle_counter_right_motor variable by 1 every time the routine is executed
    if (throttle_counter_right_motor > throttle_right_motor_memory)
    {                                                     //If the number of loops is larger then the throttle_right_motor_memory variable
      throttle_counter_right_motor = 0;                   //Reset the throttle_counter_right_motor variable
      throttle_right_motor_memory = throttle_right_motor; //Load the next throttle_right_motor variable
      if (throttle_right_motor_memory < 0)
      {                                    //If the throttle_right_motor_memory is negative
        digitalWrite(DIR_2, BWD);          //Set output 5 low to reverse the direction of the stepper controller
        throttle_right_motor_memory *= -1; //Invert the throttle_right_motor_memory variable
      }
      else
        digitalWrite(DIR_2, FWD);
      ; //Set output 5 high for a forward direction of the stepper motor
    }
    else if (throttle_counter_right_motor == 1)
      digitalWrite(STEP_2, HIGH); //Set output 4 high to create a pulse for the stepper controller
    else if (throttle_counter_right_motor == 2)
      digitalWrite(STEP_2, LOW);

    delay(1);
  }
}