#include <Arduino.h>
#include <Wire.h>
#include <MPU6050_tockn.h>
//TaskHandle_t TareaProcesador1;
//void loop1(void *pvParameters);
/*
=========================================
definiciones  de los motores paso a paso
=========================================
*/
#define STEP_1 13
#define DIR_1 25
#define STEP_2 33
#define DIR_2 14
#define max_speed 100
void IRAM_ATTR onTimer(); //declaración callback interrupciones tick Motores
void motores_calc();
void control_on_off(float angulo);
hw_timer_t *timer_motores = NULL; //timer para controlar tick de los motores paso a paso
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
volatile int tick_motores;

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 float setPoint = 0.0;
volatile int dir = 0;
bool FWD = HIGH;
bool BWD = LOW;
//==========================================
/*
============================================
Definiciones MPU6050
============================================
*/
MPU6050 mpu6050(Wire);
float ay;
long timer = 0;
//==========================================

void setup()
{
  //Configuracion motores
  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);
  timer_motores = timerBegin(0, 80, true);
  timerAttachInterrupt(timer_motores, &onTimer, true);
  timerAlarmWrite(timer_motores, 50, true);

  //I2C
  Wire.begin();
  Serial.begin(115200);
  Wire.begin();
  // xTaskCreatePinnedToCore(loop1, "Tarea1", 10000, NULL, 1, &TareaProcesador1, 0);

  delay(1000); // Pause for 1 seconds

  //Configuracion MPU6050
  mpu6050.begin();
  mpu6050.calcGyroOffsets(false);
  delay(2000);

  //Calculamos el setPoint inicial
  Serial.print("\n[");
  for (int i = 0; i < 100; i++)
  {
    mpu6050.update();
    setPoint += mpu6050.getAngleY();
    //Serial.print(setPoint),Serial.print(",");
    delay(100);
  }
  Serial.print("]");
  setPoint = setPoint / 100;
  Serial.print("\nSetpoint:"), Serial.println(setPoint);
  Serial.print("Core: "), Serial.println(xPortGetCoreID());

  timerAlarmEnable(timer_motores);
}

void loop()
{

  if (millis() - timer > 4)
  {
    mpu6050.update();
    ay = mpu6050.getAngleY();

    if (ay > 30 || ay < -30)
    {
      throttle_left_motor = 0;
      throttle_right_motor = 0;
    }
    else
    {
      control_on_off(ay);
    }

    //Serial.println(ay);*/

    timer = millis();
  }
  if (tick_motores > 0)
  {
    motores_calc();
  }
}

/*
=============================
Funciones Motores paso a paso
==============================
*/
//Interrupcion Tick Motores paso a paso
void IRAM_ATTR onTimer()
{
  portENTER_CRITICAL_ISR(&timerMux);
  tick_motores++;
  portEXIT_CRITICAL_ISR(&timerMux);
}

void motores_calc()
{
  portENTER_CRITICAL(&timerMux);
  tick_motores--;
  portEXIT_CRITICAL(&timerMux);
  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);
}

void control_on_off(float angulo)
{

  float error = angulo - setPoint;
  if (error > 4)
  {
    dir = -1;
  }
  else if (error < -4)
  {
    dir = 1;
  }
  else
  {
    dir = 0;
  }
  throttle_left_motor = max_speed * dir;
  throttle_right_motor = max_speed * dir;
}

/*void loop1(void *pvParameters){
  for(;;){
  Serial.print("Core: "),Serial.println(xPortGetCoreID());   
  }
}*/