#include <BlynkSimpleEsp32.h>
#include <WiFi.h>
#include <WiFiClient.h>
#include "FastAccelStepper.h"

//Stepper
#define dirPinStepper 21
#define stepPinStepper 19

FastAccelStepperEngine engine = FastAccelStepperEngine();
FastAccelStepper *stepper = NULL;

//Blynk thing
#define ssid  "Wokwi-GUEST"
#define pass  ""
#define BLYNK_AUTH_TOKEN    "ybh1hSWz3f7-eRlDsWoJPYgAh75O2duO"
#define BLYNK_PRINT Serial
WidgetLCD lcd(V5);
//____________________________________________________________

WidgetLED led1(V8);
const int mi = 4; // microstep

int pinV0, firstPos, holePos, lenTube, wire, motorSpeed, pinV7;
/*
V0 = on/off
V1 = vi tri ban dau
V2 = vi tri lo
V3 = Chieu dai lo
V4 = Day

V5 = LCD widget
V6 = speed
V7 = correct position
*/

bool isFirstConnect = true;
int targetPosition = 1;

BLYNK_CONNECTED() {
  Blynk.syncAll();
  if (isFirstConnect) {
    Blynk.virtualWrite(V0, 0);
    isFirstConnect = false;
  }
}

void printNum(int x, int y, int number) {
  //x = tu trai qua
  char myNumberToPrint[4] = "";
  snprintf(myNumberToPrint, sizeof(myNumberToPrint), "%3d", number);
  lcd.print(x, y, myNumberToPrint);
}

BLYNK_WRITE(V0) { //Start / Stop button
  pinV0 = param.asInt();
  Serial.println(pinV0);

  if ((pinV0 == 1) && (!stepper->isRunning())) {
    Serial.println("Motor is runnning");
    led1.on();
  }        
  
  if ((pinV0 == 0) && (stepper->isRunning())) {
    Serial.println("Motor is force stopped");
    stepper->forceStop();
    led1.off();
  }
  lcd.print(15, 0, pinV0);  
}

BLYNK_WRITE(V1) {
  firstPos = param.asInt();
  Serial.println(firstPos);
  lcd.print(0, 0, "          ");
  printNum(0, 0, firstPos);
}

BLYNK_WRITE(V2) {
  holePos = param.asInt();
  Serial.println(holePos);
  printNum(0, 1, holePos);
}

BLYNK_WRITE(V3) {
  lenTube = param.asInt();
  targetPosition = 200 * mi * lenTube;
  printNum(6, 1, lenTube);
}

BLYNK_WRITE(V4) {
  wire = param.asInt();
  Serial.println(wire);
  //targetPosition = lenTube / wire;
  printNum(6, 0, wire);
}

BLYNK_WRITE(V6) {
  motorSpeed = param.asInt();
  printNum(10, 0, motorSpeed); 

}

BLYNK_WRITE(V7) {
  pinV7 = param.asInt();
  Serial.println(pinV7);
  if (pinV7 == 1) {
    
    if (stepper->isRunning()) Serial.println("Motor dang chay.");
    else Serial.println("Motor dang dung.");

  //stepper->moveTo(int(firstPos - holePos) / wire)), true);
  //stepper->setCurrentPosition(int(holePos / wire));
  //stepper->moveTo(0);
  //stepper->forceStop();

  }

  lcd.print(15, 1, pinV7);
}

void setup() {
  Serial.begin(9600);
  delay(1000);

  Blynk.begin(BLYNK_AUTH_TOKEN, ssid, pass);
  if (Blynk.connected()) {
    Serial.println("Connected to Blynk server");
    lcd.print(0, 0, "Da ket noi");    
  }

  engine.init();
  stepper = engine.stepperConnectToPin(stepPinStepper);
  stepper->setDirectionPin(dirPinStepper);
  stepper->setAutoEnable(true);

  stepper->setSpeedInHz(mi * 600);
  stepper->setAcceleration(mi * 200);
  //stepper->moveTo(8000, false);

}

void loop() {
  Blynk.run();
  rollingthedeep();
}

void rollingthedeep() {
  if ((!stepper->isRunning()) && (pinV0 == 1)) {
    
    Serial.println("Runnning");
    stepper->setSpeedInHz(mi * 600);

    int32_t pos = stepper->getCurrentPosition();
    if (pos <= 0) {
      stepper->moveTo(targetPosition);
    } else if (pos >= targetPosition) {
      stepper->moveTo(0);
    } else {
      stepper->moveTo(stepper->targetPos());
    }
  }
}